[AutoBalancer.cpp, GaitGenerator.*] extend contactStates, controlSwin…

…gSupportTime and limbCOPOffset for arms

rtc/AutoBalancer/AutoBalancer.cpp

@@ -597,46 +597,69 @@ void AutoBalancer::getTargetParameters()
  }
  }
  gg->get_swing_support_mid_coords(tmp_fix_coords);
- // TODO : assume biped
- switch (gg->get_current_support_states().front()) {
- case BOTH:
- m_contactStates.data[contact_states_index_map["rleg"]] = true;
- m_contactStates.data[contact_states_index_map["lleg"]] = true;
- break;
- case RLEG:
- m_contactStates.data[contact_states_index_map["rleg"]] = true;
- m_contactStates.data[contact_states_index_map["lleg"]] = false;
- break;
- case LLEG:
- m_contactStates.data[contact_states_index_map["rleg"]] = false;
- m_contactStates.data[contact_states_index_map["lleg"]] = true;
- break;
- default:
- if (DEBUGP) std::cerr << "not implemented yet " << std::endl;
- break;
+ // set contactStates
+ {
+ std::vector<std::string> tmp_current_support_states_names;
+ {
+ std::vector<leg_type> tmp_current_support_states = gg->get_current_support_states();
+ std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+ for (std::vector<leg_type>::const_iterator it = tmp_current_support_states.begin(); it != tmp_current_support_states.end(); it++)
+ tmp_current_support_states_names.push_back(leg_type_map[*it]);
+ }
+ for (std::vector<std::string>::const_iterator it = leg_names.begin(); it != leg_names.end(); it++) {
+ std::vector<std::string>::const_iterator dst = std::find_if(tmp_current_support_states_names.begin(), tmp_current_support_states_names.end(), boost::lambda::_1 == *it);
+ if (dst != tmp_current_support_states_names.end()) {
+ m_contactStates.data[contact_states_index_map[*it]] = true;
+ } else {
+ m_contactStates.data[contact_states_index_map[*it]] = false;
+ }
+ }
+ }
+ // set controlSwingSupportTime
+ {
+ std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+ for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
+ std::map<leg_type, std::string>::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == it->first));
+ m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = gg->get_current_swing_time(dst->first);
+ }
+ }
+ // set limbCOPOffset
+ {
+ std::vector<std::string> swg_leg_nms = gg->get_swing_leg_names();
+ for (size_t i = 0; i < swg_leg_nms.size(); i++) {
+ m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.x = gg->get_swing_foot_zmp_offsets().at(i)(0);
+ m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.y = gg->get_swing_foot_zmp_offsets().at(i)(1);
+ m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.z = gg->get_swing_foot_zmp_offsets().at(i)(2);
+ }
+ }
+ {
+ std::vector<std::string> sup_leg_nms = gg->get_support_leg_names();
+ for (size_t i = 0; i < sup_leg_nms.size(); i++) {
+ m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.x = gg->get_support_foot_zmp_offsets().at(i)(0);
+ m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.y = gg->get_support_foot_zmp_offsets().at(i)(1);
+ m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.z = gg->get_support_foot_zmp_offsets().at(i)(2);
+ }
  }
- m_controlSwingSupportTime.data[contact_states_index_map["rleg"]] = gg->get_current_swing_time(0);
- m_controlSwingSupportTime.data[contact_states_index_map["lleg"]] = gg->get_current_swing_time(1);
- m_limbCOPOffset[contact_states_index_map[gg->get_swing_leg_names().front()]].data.x = gg->get_swing_foot_zmp_offsets().front()(0);
- m_limbCOPOffset[contact_states_index_map[gg->get_swing_leg_names().front()]].data.y = gg->get_swing_foot_zmp_offsets().front()(1);
- m_limbCOPOffset[contact_states_index_map[gg->get_swing_leg_names().front()]].data.z = gg->get_swing_foot_zmp_offsets().front()(2);
- m_limbCOPOffset[contact_states_index_map[gg->get_support_leg_names().front()]].data.x = gg->get_support_foot_zmp_offsets().front()(0);
- m_limbCOPOffset[contact_states_index_map[gg->get_support_leg_names().front()]].data.y = gg->get_support_foot_zmp_offsets().front()(1);
- m_limbCOPOffset[contact_states_index_map[gg->get_support_leg_names().front()]].data.z = gg->get_support_foot_zmp_offsets().front()(2);
  } else {
  tmp_fix_coords = fix_leg_coords;
  // double support by default
- m_contactStates.data[contact_states_index_map["rleg"]] = true;
- m_contactStates.data[contact_states_index_map["lleg"]] = true;
- // controlSwingSupportTime is not used while double support period, 1.0 is neglected
- m_controlSwingSupportTime.data[contact_states_index_map["rleg"]] = 1.0;
- m_controlSwingSupportTime.data[contact_states_index_map["lleg"]] = 1.0;
- m_limbCOPOffset[contact_states_index_map["rleg"]].data.x = default_zmp_offsets[0](0);
- m_limbCOPOffset[contact_states_index_map["rleg"]].data.y = default_zmp_offsets[0](1);
- m_limbCOPOffset[contact_states_index_map["rleg"]].data.z = default_zmp_offsets[0](2);
- m_limbCOPOffset[contact_states_index_map["lleg"]].data.x = default_zmp_offsets[1](0);
- m_limbCOPOffset[contact_states_index_map["lleg"]].data.y = default_zmp_offsets[1](1);
- m_limbCOPOffset[contact_states_index_map["lleg"]].data.z = default_zmp_offsets[1](2);
+ {
+ std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+ for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
+ std::vector<std::string>::const_iterator dst = std::find_if(leg_names.begin(), leg_names.end(), (boost::lambda::_1 == it->first));
+ if (dst != leg_names.end()) {
+ m_contactStates.data[contact_states_index_map[it->first]] = true;
+ } else {
+ m_contactStates.data[contact_states_index_map[it->first]] = false;
+ }
+ // controlSwingSupportTime is not used while double support period, 1.0 is neglected
+ m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = 1.0;
+ std::map<leg_type, std::string>::const_iterator dst2 = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == it->first));
+ m_limbCOPOffset[contact_states_index_map[it->first]].data.x = default_zmp_offsets.at(dst2->first)(0);
+ m_limbCOPOffset[contact_states_index_map[it->first]].data.y = default_zmp_offsets.at(dst2->first)(1);
+ m_limbCOPOffset[contact_states_index_map[it->first]].data.z = default_zmp_offsets.at(dst2->first)(2);
+ }
+ }
  }
  if (!adjust_footstep_interpolator->isEmpty()) {
  double tmp = 0.0;

rtc/AutoBalancer/GaitGenerator.cpp

@@ -258,8 +258,12 @@ namespace rats
  swing_ratio = static_cast<double>(current_swing_count-support_len/2)/swing_len;
  //std::cerr << "gp " << swing_ratio << " " << swing_rot_ratio << std::endl;
  }
- current_swing_time[support_leg_types.front()] = (lcg_count + 0.5 * default_double_support_ratio * next_one_step_count) * dt;
- current_swing_time[support_leg_types.front()==RLEG ? LLEG : RLEG] = tmp_current_swing_time;
+ for (std::vector<leg_type>::const_iterator it = support_leg_types.begin(); it != support_leg_types.end(); it++) {
+ current_swing_time.at(*it) = (lcg_count + 0.5 * default_double_support_ratio * next_one_step_count) * dt;
+ }
+ for (std::vector<leg_type>::const_iterator it = swing_leg_types.begin(); it != swing_leg_types.end(); it++) {
+ current_swing_time.at(*it) = tmp_current_swing_time;
+ }
  //std::cerr << "sl " << support_leg << " " << current_swing_time[support_leg==RLEG?0:1] << " " << current_swing_time[support_leg==RLEG?1:0] << " " << tmp_current_swing_time << " " << lcg_count << std::endl;
  };
 

rtc/AutoBalancer/GaitGenerator.h

@@ -528,7 +528,8 @@ namespace rats
  std::vector<step_node> support_leg_steps;
  // Swing leg coordinates is interpolated from swing_leg_src_coords to swing_leg_dst_coords during swing phase.
  std::vector<step_node> swing_leg_steps, swing_leg_src_steps, swing_leg_dst_steps;
- double default_step_height, default_top_ratio, current_step_height, swing_ratio, swing_rot_ratio, foot_midcoords_ratio, dt, current_swing_time[2], current_toe_angle, current_heel_angle;
+ double default_step_height, default_top_ratio, current_step_height, swing_ratio, swing_rot_ratio, foot_midcoords_ratio, dt, current_toe_angle, current_heel_angle;
+ std::vector<double> current_swing_time;
  // Index for current footstep. footstep_index should be [0,footstep_node_list.size()]. Current footstep is footstep_node_list[footstep_index].
  size_t footstep_index;
  // one_step_count is total counter num of current steps (= step_time/dt). lcg_count is counter for lcg. During one step, lcg_count decreases from one_step_count to 0.
@@ -576,6 +577,7 @@ namespace rats
  {
  support_leg_types = boost::assign::list_of<leg_type>(RLEG);
  swing_leg_types = boost::assign::list_of<leg_type>(LLEG);
+ current_swing_time = boost::assign::list_of<double>(0.0)(0.0)(0.0)(0.0);
  rdtg.set_dt(dt);
  sdtg.set_dt(dt);
  cdtg.set_dt(dt);
@@ -703,7 +705,7 @@ namespace rats
  void update_leg_steps (const std::vector< std::vector<step_node> >& fnsl, const double default_double_support_ratio);
  size_t get_footstep_index() const { return footstep_index; };
  size_t get_lcg_count() const { return lcg_count; };
- double get_current_swing_time(const size_t idx) const { return current_swing_time[idx]; };
+ double get_current_swing_time(const size_t idx) const { return current_swing_time.at(idx); };
  const std::vector<step_node>& get_swing_leg_steps() const { return swing_leg_steps; };
  const std::vector<step_node>& get_support_leg_steps() const { return support_leg_steps; };
  const std::vector<step_node>& get_swing_leg_src_steps() const { return swing_leg_src_steps; };
@@ -733,23 +735,14 @@ namespace rats
  };
  std::vector<leg_type> get_current_support_states () const
  {
- if ( current_step_height > 0.0 ) {
- if ( 0.0 < swing_ratio && swing_ratio < 1.0 ) {
- return get_support_leg_types();
- } else {
- if (get_support_leg_types().size() == 1) {
- return boost::assign::list_of(BOTH);
- } else {
- return boost::assign::list_of(ALL);
- }
- }
- } else {
- if (get_support_leg_types().size() == 1) {
- return boost::assign::list_of(BOTH);
+ if ( current_step_height > 0.0 && 0.0 < swing_ratio && swing_ratio < 1.0 ) {
+ return get_support_leg_types();
  } else {
- return boost::assign::list_of(ALL);
+ std::vector<leg_type> tmp_sup_types = get_support_leg_types();
+ std::vector<leg_type> tmp_swg_types = get_swing_leg_types();
+ std::copy(tmp_swg_types.begin(), tmp_swg_types.end(), std::back_inserter(tmp_sup_types));
+ return tmp_sup_types;
  }
- }
  };
  orbit_type get_default_orbit_type () const { return default_orbit_type; };
  double get_swing_trajectory_delay_time_offset () { return rdtg.get_swing_trajectory_delay_time_offset(); };