Merge pull request #897 from eisoku9618/fix-fixed-coords

Fix fixed coords
fkanehiro · Nov 29, 2015 · 45354ec · 45354ec
2 parents 5a4bdc7 + 97e91ef
commit 45354ec
Showing 1 changed file with 26 additions and 15 deletions.
diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -894,9 +894,11 @@ void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matri
   // get current foot mid pos + rot
   std::vector<coordinates> foot_coords;
   for (size_t i = 0; i < leg_names.size(); i++) {
-      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)));
+      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)));
+      }
   }
   coordinates current_foot_mid_coords;
   multi_mid_coords(current_foot_mid_coords, foot_coords);
@@ -1097,7 +1099,9 @@ void AutoBalancer::stopWalking ()
 {
   std::vector<coordinates> tmp_end_coords_list;
   for (std::vector<string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) {
-      tmp_end_coords_list.push_back(ikp[*it].target_end_coords);
+      if ((*it).find("leg") != std::string::npos) {
+          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);
@@ -1495,17 +1499,24 @@ bool AutoBalancer::setAutoBalancerParam(const OpenHRP::AutoBalancerService::Auto
                                                                           i_param.graspless_manip_reference_trans_rot[2],
                                                                           i_param.graspless_manip_reference_trans_rot[3]).normalized().toRotationMatrix()); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
   transition_time = i_param.transition_time;
-  if (leg_names_interpolator->isEmpty()) {
-      leg_names.clear();
-      for (size_t i = 0; i < i_param.leg_names.length(); i++) {
-          leg_names.push_back(std::string(i_param.leg_names[i]));
-      }
-      if (control_mode == MODE_ABC) {
-          double tmp_ratio = 0.0;
-          leg_names_interpolator->set(&tmp_ratio);
-          tmp_ratio = 1.0;
-          leg_names_interpolator->go(&tmp_ratio, 5.0, true);
-          control_mode = MODE_SYNC_TO_ABC;
+  std::vector<std::string> cur_leg_names, dst_leg_names;
+  cur_leg_names = leg_names;
+  for (size_t i = 0; i < i_param.leg_names.length(); i++) {
+      dst_leg_names.push_back(std::string(i_param.leg_names[i]));
+  }
+  std::sort(cur_leg_names.begin(), cur_leg_names.end());
+  std::sort(dst_leg_names.begin(), dst_leg_names.end());
+  if (cur_leg_names != dst_leg_names) {
+      if (leg_names_interpolator->isEmpty()) {
+          leg_names.clear();
+          leg_names = dst_leg_names;
+          if (control_mode == MODE_ABC) {
+              double tmp_ratio = 0.0;
+              leg_names_interpolator->set(&tmp_ratio);
+              tmp_ratio = 1.0;
+              leg_names_interpolator->go(&tmp_ratio, 5.0, true);
+              control_mode = MODE_SYNC_TO_ABC;
+          }
       }
   } else {
       std::cerr << "[" << m_profile.instance_name << "]   leg_names cannot be set because interpolating." << std::endl;