From 7cc36e0741a3a167f97cd3e93eef080fbaaea7ee Mon Sep 17 00:00:00 2001 From: eisoku9618 Date: Sat, 15 Aug 2015 10:45:30 +0900 Subject: [PATCH 1/7] [GaitGenerator.*] move get_support_leg_types_from_footstep_nodes to gait_generator --- rtc/AutoBalancer/GaitGenerator.cpp | 54 +++++++++++++++--------------- rtc/AutoBalancer/GaitGenerator.h | 2 +- 2 files changed, 28 insertions(+), 28 deletions(-) diff --git a/rtc/AutoBalancer/GaitGenerator.cpp b/rtc/AutoBalancer/GaitGenerator.cpp index 9f383e52153..788a70c9e1d 100644 --- a/rtc/AutoBalancer/GaitGenerator.cpp +++ b/rtc/AutoBalancer/GaitGenerator.cpp @@ -35,33 +35,6 @@ namespace rats ret = dvm * cycloid_point + start + uz; }; - std::vector get_support_leg_types_from_footstep_nodes(const std::vector& fns, std::vector _all_limbs) { - std::vector fns_names, cntr_legs_names; - std::vector ret; - for (size_t i = 0; i < fns.size(); i++) { - switch (fns.at(i).l_r) { - case RLEG: fns_names.push_back("rleg"); break; - case LLEG: fns_names.push_back("lleg"); break; - case RARM: fns_names.push_back("rarm"); break; - case LARM: fns_names.push_back("larm"); break; - default: std::cerr << "invalid input" << std::endl; - } - } - std::sort(_all_limbs.begin(), _all_limbs.end()); - std::sort(fns_names.begin(), fns_names.end()); - std::set_difference(_all_limbs.begin(), _all_limbs.end(), /* all candidates for legs */ - fns_names.begin(), fns_names.end(), /* support legs */ - std::back_inserter(cntr_legs_names)); /* swing legs */ - for (size_t i = 0; i < cntr_legs_names.size(); i++) { - if (cntr_legs_names.at(i) == "rleg") ret.push_back(RLEG); - else if (cntr_legs_names.at(i) == "lleg") ret.push_back(LLEG); - else if (cntr_legs_names.at(i) == "rarm") ret.push_back(RARM); - else if (cntr_legs_names.at(i) == "larm") ret.push_back(LARM); - else std::cerr << "invalid input" << std::endl; - } - return ret; - }; - /* member function implementation for refzmp_generator */ void refzmp_generator::push_refzmp_from_footstep_nodes_for_dual (const std::vector& fns, const std::vector& _support_leg_steps, @@ -757,5 +730,32 @@ namespace rats rg.update_refzmp(footstep_nodes_list); } }; + + std::vector gait_generator::get_support_leg_types_from_footstep_nodes(const std::vector& fns, std::vector _all_limbs) { + std::vector fns_names, cntr_legs_names; + std::vector ret; + for (size_t i = 0; i < fns.size(); i++) { + switch (fns.at(i).l_r) { + case RLEG: fns_names.push_back("rleg"); break; + case LLEG: fns_names.push_back("lleg"); break; + case RARM: fns_names.push_back("rarm"); break; + case LARM: fns_names.push_back("larm"); break; + default: std::cerr << "invalid input" << std::endl; + } + } + std::sort(_all_limbs.begin(), _all_limbs.end()); + std::sort(fns_names.begin(), fns_names.end()); + std::set_difference(_all_limbs.begin(), _all_limbs.end(), /* all candidates for legs */ + fns_names.begin(), fns_names.end(), /* support legs */ + std::back_inserter(cntr_legs_names)); /* swing legs */ + for (size_t i = 0; i < cntr_legs_names.size(); i++) { + if (cntr_legs_names.at(i) == "rleg") ret.push_back(RLEG); + else if (cntr_legs_names.at(i) == "lleg") ret.push_back(LLEG); + else if (cntr_legs_names.at(i) == "rarm") ret.push_back(RARM); + else if (cntr_legs_names.at(i) == "larm") ret.push_back(LARM); + else std::cerr << "invalid input" << std::endl; + } + return ret; + }; } diff --git a/rtc/AutoBalancer/GaitGenerator.h b/rtc/AutoBalancer/GaitGenerator.h index ff3df60a6f0..da230799dc4 100644 --- a/rtc/AutoBalancer/GaitGenerator.h +++ b/rtc/AutoBalancer/GaitGenerator.h @@ -55,7 +55,6 @@ namespace rats return os; }; }; - std::vector get_support_leg_types_from_footstep_nodes(const std::vector& fns, std::vector _all_limbs); /* footstep parameter */ struct footstep_parameter @@ -1050,6 +1049,7 @@ namespace rats int get_NUM_TH_PHASES () { return thp.get_NUM_TH_PHASES(); }; bool get_use_toe_joint () { return lcg.get_use_toe_joint(); }; void get_leg_default_translate_pos (std::vector& off) { off = footstep_param.leg_default_translate_pos; }; + std::vector get_support_leg_types_from_footstep_nodes(const std::vector& fns, std::vector _all_limbs); 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; }; void print_param (const std::string& print_str = "") From 554382e1949fd3cdf568387bb5c6e229c1ae064e Mon Sep 17 00:00:00 2001 From: eisoku9618 Date: Sat, 15 Aug 2015 10:58:47 +0900 Subject: [PATCH 2/7] [GaitGenerator.*] rename get_support_leg_types_from_footstep_nodes to calc_counter_leg_types_from_footstep_nodes --- rtc/AutoBalancer/GaitGenerator.cpp | 12 ++++++------ rtc/AutoBalancer/GaitGenerator.h | 2 +- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/rtc/AutoBalancer/GaitGenerator.cpp b/rtc/AutoBalancer/GaitGenerator.cpp index 788a70c9e1d..1cf28a13b1a 100644 --- a/rtc/AutoBalancer/GaitGenerator.cpp +++ b/rtc/AutoBalancer/GaitGenerator.cpp @@ -563,19 +563,19 @@ namespace rats dr = start_ref_coords.rot.transpose() * dr; } for (size_t i = 0; i < optional_go_pos_finalize_footstep_num; i++) { - append_go_pos_step_nodes(start_ref_coords, get_support_leg_types_from_footstep_nodes(footstep_nodes_list.back(), all_limbs)); + append_go_pos_step_nodes(start_ref_coords, calc_counter_leg_types_from_footstep_nodes(footstep_nodes_list.back(), all_limbs)); } /* finalize */ // Align last foot - append_go_pos_step_nodes(start_ref_coords, get_support_leg_types_from_footstep_nodes(footstep_nodes_list.back(), all_limbs)); + append_go_pos_step_nodes(start_ref_coords, calc_counter_leg_types_from_footstep_nodes(footstep_nodes_list.back(), all_limbs)); // Check align coordinates final_step_coords1 = footstep_nodes_list[footstep_nodes_list.size()-2].front().worldcoords; // Final coords in footstep_node_list coordinates final_step_coords2 = start_ref_coords; // Final coords calculated from start_ref_coords + translate pos final_step_coords2.pos += final_step_coords2.rot * hrp::Vector3(footstep_param.leg_default_translate_pos[footstep_nodes_list[footstep_nodes_list.size()-2].front().l_r]); final_step_coords1.difference(dp, dr, final_step_coords2); if ( !(eps_eq(dp.norm(), 0.0, 1e-3*0.1) && eps_eq(dr.norm(), 0.0, deg2rad(0.5))) ) { // If final_step_coords1 != final_step_coords2, add steps to match final_step_coords1 and final_step_coords2 - append_go_pos_step_nodes(start_ref_coords, get_support_leg_types_from_footstep_nodes(footstep_nodes_list.back(), all_limbs)); + append_go_pos_step_nodes(start_ref_coords, calc_counter_leg_types_from_footstep_nodes(footstep_nodes_list.back(), all_limbs)); } // For Last double support period append_finalize_footstep(); @@ -653,7 +653,7 @@ namespace rats ref_coords.pos += ref_coords.rot * trans; ref_coords.rotate(dth, hrp::Vector3(0,0,1)); - append_go_pos_step_nodes(ref_coords, get_support_leg_types_from_footstep_nodes(footstep_nodes_list.back(), all_limbs)); + append_go_pos_step_nodes(ref_coords, calc_counter_leg_types_from_footstep_nodes(footstep_nodes_list.back(), all_limbs)); }; void gait_generator::calc_next_coords_velocity_mode (std::vector< std::vector >& ret_list, const size_t idx) @@ -665,7 +665,7 @@ namespace rats std::vector cur_sup_legs, next_sup_legs; for (size_t i = 0; i < footstep_nodes_list[idx-1].size(); i++) cur_sup_legs.push_back(footstep_nodes_list[idx-1].at(i).l_r); - next_sup_legs = get_support_leg_types_from_footstep_nodes(footstep_nodes_list[idx-1], all_limbs); + next_sup_legs = calc_counter_leg_types_from_footstep_nodes(footstep_nodes_list[idx-1], all_limbs); for (size_t i = 0; i < 3; i++) { std::vector ret; @@ -731,7 +731,7 @@ namespace rats } }; - std::vector gait_generator::get_support_leg_types_from_footstep_nodes(const std::vector& fns, std::vector _all_limbs) { + const std::vector gait_generator::calc_counter_leg_types_from_footstep_nodes(const std::vector& fns, std::vector _all_limbs) const { std::vector fns_names, cntr_legs_names; std::vector ret; for (size_t i = 0; i < fns.size(); i++) { diff --git a/rtc/AutoBalancer/GaitGenerator.h b/rtc/AutoBalancer/GaitGenerator.h index da230799dc4..61d70fbe8ac 100644 --- a/rtc/AutoBalancer/GaitGenerator.h +++ b/rtc/AutoBalancer/GaitGenerator.h @@ -1049,7 +1049,7 @@ namespace rats int get_NUM_TH_PHASES () { return thp.get_NUM_TH_PHASES(); }; bool get_use_toe_joint () { return lcg.get_use_toe_joint(); }; void get_leg_default_translate_pos (std::vector& off) { off = footstep_param.leg_default_translate_pos; }; - std::vector get_support_leg_types_from_footstep_nodes(const std::vector& fns, std::vector _all_limbs); + const std::vector calc_counter_leg_types_from_footstep_nodes (const std::vector& fns, std::vector _all_limbs) const; 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; }; void print_param (const std::string& print_str = "") From fcb255cb35342039db328c73cd9f9f00e26e4056 Mon Sep 17 00:00:00 2001 From: eisoku9618 Date: Sat, 15 Aug 2015 11:01:30 +0900 Subject: [PATCH 3/7] [AutoBalancer.cpp, GaitGenerator.h] move leg_type_map to gait_generator --- rtc/AutoBalancer/AutoBalancer.cpp | 4 ++-- rtc/AutoBalancer/GaitGenerator.h | 3 +++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp index b6c01872203..4e1aaf4427b 100644 --- a/rtc/AutoBalancer/AutoBalancer.cpp +++ b/rtc/AutoBalancer/AutoBalancer.cpp @@ -554,9 +554,9 @@ void AutoBalancer::getTargetParameters() gg->set_default_zmp_offsets(default_zmp_offsets); gg_solved = gg->proc_one_tick(); { - // for support leg - std::map leg_type_map = boost::assign::map_list_of(RLEG, "rleg")(LLEG, "lleg")(RARM, "rarm")(LARM, "larm"); + std::map leg_type_map = gg->get_leg_type_map(); coordinates tmpc; + // for support leg for (std::vector::const_iterator it = gg->get_support_leg_steps().begin(); it != gg->get_support_leg_steps().end(); it++) { coordinates sp_coords = it->worldcoords; coordinates(ikp[leg_type_map[it->l_r]].localPos, diff --git a/rtc/AutoBalancer/GaitGenerator.h b/rtc/AutoBalancer/GaitGenerator.h index 61d70fbe8ac..20921b1e1db 100644 --- a/rtc/AutoBalancer/GaitGenerator.h +++ b/rtc/AutoBalancer/GaitGenerator.h @@ -758,6 +758,7 @@ namespace rats velocity_mode_flag velocity_mode_flg; emergency_flag emergency_flg; bool use_inside_step_limitation; + std::map leg_type_map; /* preview controller parameters */ //preview_dynamics_filter* preview_controller_ptr; @@ -800,6 +801,7 @@ namespace rats 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()); + leg_type_map = boost::assign::map_list_of(RLEG, "rleg")(LLEG, "lleg")(RARM, "rarm")(LARM, "larm"); }; ~gait_generator () { if ( preview_controller_ptr != NULL ) { @@ -1050,6 +1052,7 @@ namespace rats bool get_use_toe_joint () { return lcg.get_use_toe_joint(); }; void get_leg_default_translate_pos (std::vector& off) { off = footstep_param.leg_default_translate_pos; }; const std::vector calc_counter_leg_types_from_footstep_nodes (const std::vector& fns, std::vector _all_limbs) const; + const std::map get_leg_type_map () const { return leg_type_map; }; 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; }; void print_param (const std::string& print_str = "") From aff8ca9b49584566b1e4929974b416d354649c2c Mon Sep 17 00:00:00 2001 From: eisoku9618 Date: Sat, 15 Aug 2015 12:01:31 +0900 Subject: [PATCH 4/7] [GaitGenerator.*] use leg_type_map in order to convert between leg_type and name --- rtc/AutoBalancer/GaitGenerator.cpp | 23 +++++++---------------- rtc/AutoBalancer/GaitGenerator.h | 10 ++-------- 2 files changed, 9 insertions(+), 24 deletions(-) diff --git a/rtc/AutoBalancer/GaitGenerator.cpp b/rtc/AutoBalancer/GaitGenerator.cpp index 1cf28a13b1a..4b2953a8041 100644 --- a/rtc/AutoBalancer/GaitGenerator.cpp +++ b/rtc/AutoBalancer/GaitGenerator.cpp @@ -733,27 +733,18 @@ namespace rats const std::vector gait_generator::calc_counter_leg_types_from_footstep_nodes(const std::vector& fns, std::vector _all_limbs) const { std::vector fns_names, cntr_legs_names; - std::vector ret; - for (size_t i = 0; i < fns.size(); i++) { - switch (fns.at(i).l_r) { - case RLEG: fns_names.push_back("rleg"); break; - case LLEG: fns_names.push_back("lleg"); break; - case RARM: fns_names.push_back("rarm"); break; - case LARM: fns_names.push_back("larm"); break; - default: std::cerr << "invalid input" << std::endl; - } + for (std::vector::const_iterator it = fns.begin(); it != fns.end(); it++) { + fns_names.push_back(leg_type_map.find(it->l_r)->second); } std::sort(_all_limbs.begin(), _all_limbs.end()); std::sort(fns_names.begin(), fns_names.end()); std::set_difference(_all_limbs.begin(), _all_limbs.end(), /* all candidates for legs */ fns_names.begin(), fns_names.end(), /* support legs */ - std::back_inserter(cntr_legs_names)); /* swing legs */ - for (size_t i = 0; i < cntr_legs_names.size(); i++) { - if (cntr_legs_names.at(i) == "rleg") ret.push_back(RLEG); - else if (cntr_legs_names.at(i) == "lleg") ret.push_back(LLEG); - else if (cntr_legs_names.at(i) == "rarm") ret.push_back(RARM); - else if (cntr_legs_names.at(i) == "larm") ret.push_back(LARM); - else std::cerr << "invalid input" << std::endl; + std::back_inserter(cntr_legs_names)); /* swing legs */ + std::vector ret; + for (std::vector::const_iterator it = cntr_legs_names.begin(); it != cntr_legs_names.end(); it++) { + std::map::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map::value_type::second == *it)); + ret.push_back(dst->first); } return ret; }; diff --git a/rtc/AutoBalancer/GaitGenerator.h b/rtc/AutoBalancer/GaitGenerator.h index 20921b1e1db..82a15a16a6c 100644 --- a/rtc/AutoBalancer/GaitGenerator.h +++ b/rtc/AutoBalancer/GaitGenerator.h @@ -962,14 +962,8 @@ namespace rats }; std::vector convert_leg_types_to_names (const std::vector& lts) const { std::vector ret; - for (size_t i = 0; i < lts.size(); i++) { - switch(lts.at(i)) { - case RLEG : ret.push_back("rleg"); break; - case LLEG : ret.push_back("lleg"); break; - case RARM : ret.push_back("rarm"); break; - case LARM : ret.push_back("rarm"); break; - default : ret.push_back("lleg"); break; - } + for (std::vector::const_iterator it = lts.begin(); it != lts.end(); it++) { + ret.push_back(leg_type_map.find(*it)->second); } return ret; }; From dd12b6ae66c7b2a06e402bbae39cea9d7efc7f58 Mon Sep 17 00:00:00 2001 From: eisoku9618 Date: Sat, 15 Aug 2015 12:07:44 +0900 Subject: [PATCH 5/7] [AutoBalancer.cpp] use leg_names instead of "rleg" or "lleg" --- rtc/AutoBalancer/AutoBalancer.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp index 4e1aaf4427b..362a01627dd 100644 --- a/rtc/AutoBalancer/AutoBalancer.cpp +++ b/rtc/AutoBalancer/AutoBalancer.cpp @@ -406,7 +406,7 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id) rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p); } else { for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { - if (it->first == "rleg" || it->first == "lleg") { + if (std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end() ) { it->second.current_p0 = it->second.target_link->p; it->second.current_r0 = it->second.target_link->R; } @@ -668,7 +668,7 @@ void AutoBalancer::getTargetParameters() target_root_p = m_robot->rootLink()->p; target_root_R = m_robot->rootLink()->R; for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { - if ( control_mode == MODE_IDLE || it->first.find("leg") == std::string::npos ) { + if ( control_mode == MODE_IDLE || std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end() ) { it->second.target_p0 = it->second.target_link->p; it->second.target_r0 = it->second.target_link->R; } @@ -683,7 +683,7 @@ void AutoBalancer::getTargetParameters() // for foot_mid_pos tmp_foot_mid_pos += tmpikp.target_link->p + tmpikp.target_link->R * tmpikp.localPos + tmpikp.target_link->R * tmpikp.localR * default_zmp_offsets[i]; } - tmp_foot_mid_pos *= 0.5; + tmp_foot_mid_pos *= (1.0 / leg_names.size()); // { @@ -719,7 +719,7 @@ void AutoBalancer::getTargetParameters() current_root_p = target_root_p; current_root_R = target_root_R; for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { - if ( it->first.find("leg") != std::string::npos ) { + if ( std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end() ) { it->second.target_p0 = it->second.target_link->p; it->second.target_r0 = it->second.target_link->R; } From e82410413ebd0f3a1d3d5a70f28b5fdf5449ca10 Mon Sep 17 00:00:00 2001 From: eisoku9618 Date: Sat, 15 Aug 2015 13:25:18 +0900 Subject: [PATCH 6/7] [AutoBalancer.cpp, GaitGenerator.*] add multi_mid_coords function to calculate a midcoords of multi coordinates in fixLegToCoords, get_swing_support_mid_coords and stopWalking --- rtc/AutoBalancer/AutoBalancer.cpp | 20 ++++++++++++-------- rtc/AutoBalancer/GaitGenerator.cpp | 16 ++++++++++++++++ rtc/AutoBalancer/GaitGenerator.h | 19 ++++++++++++++++--- 3 files changed, 44 insertions(+), 11 deletions(-) diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp index 362a01627dd..06bcd7a8870 100644 --- a/rtc/AutoBalancer/AutoBalancer.cpp +++ b/rtc/AutoBalancer/AutoBalancer.cpp @@ -741,16 +741,16 @@ hrp::Matrix33 AutoBalancer::OrientRotationMatrix (const hrp::Matrix33& rot, cons void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matrix33& fix_rot) { // get current foot mid pos + rot - std::vector foot_pos; - std::vector foot_rot; + std::vector foot_coords; for (size_t i = 0; i < leg_names.size(); i++) { ABCIKparam& tmpikp = ikp[leg_names[i]]; - foot_pos.push_back(tmpikp.target_link->p + tmpikp.target_link->R * tmpikp.localPos); - foot_rot.push_back(tmpikp.target_link->R * tmpikp.localR); + foot_coords.push_back(coordinates((tmpikp.target_link->p + tmpikp.target_link->R * tmpikp.localPos), + (tmpikp.target_link->R * tmpikp.localR))); } - hrp::Vector3 current_foot_mid_pos ((foot_pos[0]+foot_pos[1])/2.0); - hrp::Matrix33 current_foot_mid_rot; - mid_rot(current_foot_mid_rot, 0.5, foot_rot[0], foot_rot[1]); + coordinates current_foot_mid_coords; + multi_mid_coords(current_foot_mid_coords, foot_coords); + hrp::Vector3 current_foot_mid_pos = current_foot_mid_coords.pos; + hrp::Matrix33 current_foot_mid_rot = current_foot_mid_coords.rot; // fix root pos + rot to fix "coords" = "current_foot_mid_xx" hrp::Matrix33 tmpR (fix_rot * current_foot_mid_rot.transpose()); m_robot->rootLink()->p = fix_pos + tmpR * (m_robot->rootLink()->p - current_foot_mid_pos); @@ -917,7 +917,11 @@ void AutoBalancer::startWalking () void AutoBalancer::stopWalking () { - mid_coords(fix_leg_coords, 0.5, ikp["rleg"].target_end_coords, ikp["lleg"].target_end_coords); + std::vector tmp_end_coords_list; + for (std::vector::iterator it = leg_names.begin(); it != leg_names.end(); it++) { + tmp_end_coords_list.push_back(ikp[*it].target_end_coords); + } + multi_mid_coords(fix_leg_coords, tmp_end_coords_list); fixLegToCoords(fix_leg_coords.pos, fix_leg_coords.rot); gg->clear_footstep_nodes_list(); if (return_control_mode == MODE_IDLE) stopABCparam(); diff --git a/rtc/AutoBalancer/GaitGenerator.cpp b/rtc/AutoBalancer/GaitGenerator.cpp index 4b2953a8041..7f2a2685449 100644 --- a/rtc/AutoBalancer/GaitGenerator.cpp +++ b/rtc/AutoBalancer/GaitGenerator.cpp @@ -34,6 +34,22 @@ namespace rats u(2), v(2), 1; ret = dvm * cycloid_point + start + uz; }; + void multi_mid_coords (coordinates& ret, const std::vector& cs) + { + if (cs.size() == 1) { + ret = cs.front(); + } else { + std::vector tmp_mid_coords; + double ratio = (1.0 - 1.0 / cs.size()); + for (size_t i = 1; i < cs.size(); i++) { + coordinates tmp; + mid_coords(tmp, ratio, cs.front(), cs.at(i)); + tmp_mid_coords.push_back(tmp); + } + multi_mid_coords(ret, tmp_mid_coords); + } + return; + }; /* member function implementation for refzmp_generator */ void refzmp_generator::push_refzmp_from_footstep_nodes_for_dual (const std::vector& fns, diff --git a/rtc/AutoBalancer/GaitGenerator.h b/rtc/AutoBalancer/GaitGenerator.h index 82a15a16a6c..50994309c41 100644 --- a/rtc/AutoBalancer/GaitGenerator.h +++ b/rtc/AutoBalancer/GaitGenerator.h @@ -15,6 +15,7 @@ namespace rats const double ratio, const hrp::Vector3& start, const hrp::Vector3& goal, const double height, const double default_top_ratio = 0.5); + void multi_mid_coords (coordinates& mid_coords, const std::vector& cs); enum orbit_type {SHUFFLING, CYCLOID, RECTANGLE, STAIR, CYCLOIDDELAY, CYCLOIDDELAYKICK}; enum leg_type {RLEG, LLEG, RARM, LARM, BOTH, ALL}; @@ -685,9 +686,21 @@ namespace rats double get_default_step_height () const { return default_step_height;}; void get_swing_support_mid_coords(coordinates& ret) const { - coordinates tmp; - mid_coords(tmp, foot_midcoords_ratio, swing_leg_src_steps.front().worldcoords, swing_leg_dst_steps.front().worldcoords); - mid_coords(ret, 0.5, tmp, support_leg_steps.front().worldcoords); + std::vector swg_coords, sup_coords; + for (std::vector::const_iterator it_src = swing_leg_src_steps.begin(), it_dst = swing_leg_dst_steps.begin(); + it_src != swing_leg_src_steps.end() && it_dst != swing_leg_dst_steps.end(); + it_src++, it_dst++) { + coordinates tmp; + mid_coords(tmp, foot_midcoords_ratio, it_src->worldcoords, it_dst->worldcoords); + swg_coords.push_back(tmp); + } + for (std::vector::const_iterator it = support_leg_steps.begin(); it != support_leg_steps.end(); it++) { + sup_coords.push_back(it->worldcoords); + } + coordinates tmp_swg_mid, tmp_sup_mid; + multi_mid_coords(tmp_swg_mid, swg_coords); + multi_mid_coords(tmp_sup_mid, sup_coords); + mid_coords(ret, 0.5, tmp_swg_mid, tmp_sup_mid); }; std::vector get_current_support_states () const { From 2c81daa58c2846abd32839c37f88c9906bb24e38 Mon Sep 17 00:00:00 2001 From: eisoku9618 Date: Sun, 16 Aug 2015 06:07:37 +0900 Subject: [PATCH 7/7] [GaitGenerator.cpp] fix bug: keep align the order of names and coordinates of foostep_nodes_list.front() --- rtc/AutoBalancer/GaitGenerator.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/rtc/AutoBalancer/GaitGenerator.cpp b/rtc/AutoBalancer/GaitGenerator.cpp index 7f2a2685449..b76f5c926d7 100644 --- a/rtc/AutoBalancer/GaitGenerator.cpp +++ b/rtc/AutoBalancer/GaitGenerator.cpp @@ -445,9 +445,14 @@ namespace rats /* clear all gait_parameter */ size_t one_step_len = footstep_nodes_list.front().front().step_time / dt; finalize_count = 0; - for (size_t i = 0; i < footstep_nodes_list.front().size(); i++) { - /* initial_swing_leg_dst_steps has dummy step_height, step_time, toe_angle and heel_angle. */ - footstep_nodes_list.front().at(i).worldcoords = initial_swing_leg_dst_steps.at(i).worldcoords; + for (std::vector::iterator it_fns = footstep_nodes_list.front().begin(); it_fns != footstep_nodes_list.front().end(); it_fns++) { + for (std::vector::const_iterator it_init = initial_swing_leg_dst_steps.begin(); it_init != initial_swing_leg_dst_steps.end(); it_init++) { + if (it_fns->l_r == it_init->l_r) { + /* initial_swing_leg_dst_steps has dummy step_height, step_time, toe_angle and heel_angle. */ + it_fns->worldcoords = it_init->worldcoords; + break; + } + } } rg.reset(one_step_len); rg.push_refzmp_from_footstep_nodes_for_dual(footstep_nodes_list.front(), initial_support_leg_steps, initial_swing_leg_dst_steps);