From ad4eb10d05f98aca9f243bb72a81ffba4b51dd77 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Sun, 13 Dec 2015 16:58:03 +0900 Subject: [PATCH] [rtc/AutoBalancer/AutoBalancer.cpp] Revert AutoBalancer 7a8bc6781608d4251b6c268123d99781ea4d405b change which does not pass samplerobot_auto_balancer.py test. --- rtc/AutoBalancer/AutoBalancer.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp index 6fa8ff3c2e7..04a08a163f6 100644 --- a/rtc/AutoBalancer/AutoBalancer.cpp +++ b/rtc/AutoBalancer/AutoBalancer.cpp @@ -894,11 +894,9 @@ void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matri // get current foot mid pos + rot std::vector foot_coords; for (size_t i = 0; i < leg_names.size(); i++) { - if (leg_names[i].find("leg") != std::string::npos) { - ABCIKparam& tmpikp = ikp[leg_names[i]]; - foot_coords.push_back(coordinates((tmpikp.target_link->p + tmpikp.target_link->R * tmpikp.localPos), - (tmpikp.target_link->R * tmpikp.localR))); - } + ABCIKparam& tmpikp = ikp[leg_names[i]]; + foot_coords.push_back(coordinates((tmpikp.target_link->p + tmpikp.target_link->R * tmpikp.localPos), + (tmpikp.target_link->R * tmpikp.localR))); } coordinates current_foot_mid_coords; multi_mid_coords(current_foot_mid_coords, foot_coords); @@ -1099,9 +1097,7 @@ void AutoBalancer::stopWalking () { std::vector tmp_end_coords_list; for (std::vector::iterator it = leg_names.begin(); it != leg_names.end(); it++) { - if ((*it).find("leg") != std::string::npos) { - tmp_end_coords_list.push_back(ikp[*it].target_end_coords); - } + 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);