[AutoBalancer.cpp, GaitGenerator.cpp, GaitGenerator.h] support multil…

…eg in go velocity
snozawa · Jan 2, 2016 · 85b24d0 · 85b24d0
1 parent e597cd9
commit 85b24d0
Show file tree

Hide file tree

Showing 3 changed files with 27 additions and 5 deletions.
diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -1195,12 +1195,33 @@ bool AutoBalancer::goPos(const double& x, const double& y, const double& th)
 
 bool AutoBalancer::goVelocity(const double& vx, const double& vy, const double& vth)
 {
+ gg->set_all_limbs(leg_names);
  if (gg_is_walking && gg_solved) {
  gg->set_velocity_param(vx, vy, vth);
  } else {
  coordinates ref_coords;
  mid_coords(ref_coords, 0.5, ikp["rleg"].target_end_coords, ikp["lleg"].target_end_coords);
- gg->initialize_velocity_mode(ref_coords, vx, vy, vth);
+ std::vector<leg_type> current_legs;
+ switch(gait_type) {
+ case BIPED:
+ current_legs = (vy > 0 ? boost::assign::list_of(RLEG) : boost::assign::list_of(LLEG));
+ break;
+ case TROT:
+ current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM));
+ break;
+ case PACE:
+ current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM));
+ break;
+ case CRAWL:
+ std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl;
+ return false;
+ case GALLOP:
+ /* at least one leg shoud be in contact */
+ std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl;
+ return false;
+ default: break;
+ }
+ gg->initialize_velocity_mode(ref_coords, vx, vy, vth, current_legs);
  startWalking();
  }
  return true;

diff --git a/rtc/AutoBalancer/GaitGenerator.cpp b/rtc/AutoBalancer/GaitGenerator.cpp
@@ -723,14 +723,14 @@ namespace rats
  };
 
  void gait_generator::initialize_velocity_mode (const coordinates& _ref_coords,
- const double vel_x, const double vel_y, const double vel_theta)
+ const double vel_x, const double vel_y, const double vel_theta,
+ const std::vector<leg_type>& current_legs)
  {
  velocity_mode_flg = VEL_DOING;
  /* initialize */
- leg_type current_leg = (vel_y > 0.0) ? RLEG : LLEG;
  clear_footstep_nodes_list();
  set_velocity_param (vel_x, vel_y, vel_theta);
- append_go_pos_step_nodes(_ref_coords, boost::assign::list_of(current_leg));
+ append_go_pos_step_nodes(_ref_coords, current_legs);
  append_footstep_list_velocity_mode();
  append_footstep_list_velocity_mode();
  append_footstep_list_velocity_mode();

diff --git a/rtc/AutoBalancer/GaitGenerator.h b/rtc/AutoBalancer/GaitGenerator.h
@@ -971,7 +971,8 @@ namespace rats
  const std::string& tmp_swing_leg,
  const coordinates& _support_leg_coords);
  void initialize_velocity_mode (const coordinates& _ref_coords,
- const double vel_x, const double vel_y, const double vel_theta); /* [mm/s] [mm/s] [deg/s] */
+ const double vel_x, const double vel_y, const double vel_theta, /* [mm/s] [mm/s] [deg/s] */
+ const std::vector<leg_type>& current_legs);
  void finalize_velocity_mode ();
  void append_finalize_footstep ()
  {