[idl/AutoBalancerService.idl, AutoBalancer.*, AutoBalancerService_imp…

…l.*, GaitGenerator.h] add setFootStepNodes for multiple legs

idl/AutoBalancerService.idl

@@ -27,6 +27,15 @@ module OpenHRP
  string leg;
  };
 
+ /**
+ * @struct Footsteps
+ * @brief Foot step for multi legs.
+ */
+ struct Footsteps
+ {
+ sequence<Footstep> fs;
+ };
+
  /**
  * @struct StepParam
  * @brief Step parameter for one step
@@ -43,13 +52,29 @@ module OpenHRP
  double heel_angle;
  };
 
+ /**
+ * @struct StepParams
+ * @brief Step parameters for multi step
+ */
+ struct StepParams
+ {
+ sequence<StepParam> sps;
+ };
+
  /**
  * @struct FootstepSequence
  * @brief Sequence of foot step.
  */
  typedef sequence<Footstep> FootstepSequence;
  typedef sequence<StepParam> StepParamSequence;
 
+ /**
+ * @struct FootstepsSequence
+ * @brief Sequence of foot steps.
+ */
+ typedef sequence<Footsteps> FootstepsSequence;
+ typedef sequence<StepParams> StepParamsSequence;
+
  /**
  * @enum SupportLegState
  * @brief State of support leg.
@@ -218,19 +243,19 @@ module OpenHRP
 
  /**
  * @brief Set footsteps. Returns without waiting for whole steps to be executed.
- * @param fs is sequence of FootStep structure.
+ * @param fss is sequence of FootStep structure.
  overwrite_fs_idx is index to be overwritten. overwrite_fs_idx is used only in walking.
  * @return true if set successfully, false otherwise
  */
- boolean setFootSteps(in FootstepSequence fs, in long overwrite_fs_idx);
+ boolean setFootSteps(in FootstepsSequence fss, in long overwrite_fs_idx);
 
  /**
  * @brief Set footsteps. Returns without waiting for whole steps to be executed.
- * @param fs is sequence of FootStepWithParam structure.
+ * @param fss is sequence of FootStepWithParam structure.
  overwrite_fs_idx is index to be overwritten. overwrite_fs_idx is used only in walking.
  * @return true if set successfully, false otherwise
  */
- boolean setFootStepsWithParam(in FootstepSequence fs, in StepParamSequence sps, in long overwrite_fs_idx);
+ boolean setFootStepsWithParam(in FootstepsSequence fss, in StepParamsSequence spss, in long overwrite_fs_idx);
 
  /**
  * @brief Wait for whole footsteps are executed.

rtc/AutoBalancer/AutoBalancer.cpp

@@ -1012,84 +1012,116 @@ bool AutoBalancer::releaseEmergencyStop ()
  return true;
 }
 
-bool AutoBalancer::setFootSteps(const OpenHRP::AutoBalancerService::FootstepSequence& fs, CORBA::Long overwrite_fs_idx)
+bool AutoBalancer::setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx)
 {
- OpenHRP::AutoBalancerService::StepParamSequence sps;
- sps.length(fs.length());
+ OpenHRP::AutoBalancerService::StepParamsSequence spss;
+ spss.length(fss.length());
  // If gg_is_walking is false, initial footstep will be double support. So, set 0 for step_height and toe heel angles.
  // If gg_is_walking is true, do not set to 0.
- for (size_t i = 0; i < sps.length(); i++) sps[i].step_height = ((!gg_is_walking && i==0) ? 0.0 : gg->get_default_step_height());
- for (size_t i = 0; i < sps.length(); i++) sps[i].step_time = gg->get_default_step_time();
- for (size_t i = 0; i < sps.length(); i++) sps[i].toe_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_toe_angle());
- for (size_t i = 0; i < sps.length(); i++) sps[i].heel_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_heel_angle());
- setFootStepsWithParam(fs, sps, overwrite_fs_idx);
+ for (size_t i = 0; i < spss.length(); i++) {
+ spss[i].sps.length(fss[i].fs.length());
+ for (size_t j = 0; j < spss[i].sps.length(); j++) {
+ spss[i].sps[j].step_height = ((!gg_is_walking && i==0) ? 0.0 : gg->get_default_step_height());
+ spss[i].sps[j].step_time = gg->get_default_step_time();
+ spss[i].sps[j].toe_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_default_step_height());
+ spss[i].sps[j].heel_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_default_step_height());
+ }
+ }
+ setFootStepsWithParam(fss, spss, overwrite_fs_idx);
 }
 
-bool AutoBalancer::setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepSequence& fs, const OpenHRP::AutoBalancerService::StepParamSequence& sps, CORBA::Long overwrite_fs_idx)
+bool AutoBalancer::setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx)
 {
- if (!is_stop_mode) {
- std::cerr << "[" << m_profile.instance_name << "] setFootSteps" << std::endl;
+ if (!is_stop_mode) {
+ std::cerr << "[" << m_profile.instance_name << "] setFootStepsList" << std::endl;
+
+ // Initial footstep Snapping
+ coordinates tmpfs, fstrans;
+ step_node initial_support_step, initial_input_step;
+ {
+ std::vector<step_node> initial_support_steps;
+ if (gg_is_walking) {
+ if (overwrite_fs_idx <= 0) {
+ std::cerr << "[" << m_profile.instance_name << "] Invalid overwrite index = " << overwrite_fs_idx << std::endl;
+ return false;
+ }
+ if (!gg->get_footstep_nodes_by_index(initial_support_steps, overwrite_fs_idx-1)) {
+ std::cerr << "[" << m_profile.instance_name << "] Invalid overwrite index = " << overwrite_fs_idx << std::endl;
+ return false;
+ }
+ } else {
+ // If walking, snap initial leg to current ABC foot coords.
+ for (size_t i = 0; i < fss[0].fs.length(); i++) {
+ initial_support_steps.push_back(step_node(std::string(fss[0].fs[i].leg), ikp[std::string(fss[0].fs[i].leg)].target_end_coords, 0, 0, 0, 0));
+ }
+ }
+ initial_support_step = initial_support_steps.front(); /* use only one leg for representation */
+ }
+ {
+ std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+ for (size_t i = 0; i < fss[0].fs.length(); i++) {
+ if (std::string(fss[0].fs[i].leg) == leg_type_map[initial_support_step.l_r]) {
+ coordinates tmp;
+ memcpy(tmp.pos.data(), fss[0].fs[i].pos, sizeof(double)*3);
+ tmp.rot = (Eigen::Quaternion<double>(fss[0].fs[i].rot[0], fss[0].fs[i].rot[1], fss[0].fs[i].rot[2], fss[0].fs[i].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
+ initial_input_step = step_node(std::string(fss[0].fs[i].leg), tmp, 0, 0, 0, 0);
+ }
+ }
+ }
 
- // Initial footstep Snapping
- coordinates tmpfs, initial_support_coords, initial_input_coords, fstrans;
- if (gg_is_walking) {
- if (overwrite_fs_idx <= 0) {
- std::cerr << "[" << m_profile.instance_name << "] Invalid overwrite index = " << overwrite_fs_idx << std::endl;
- return false;
+ // Get footsteps
+ std::vector< std::vector<coordinates> > fs_vec_list;
+ std::vector< std::vector<std::string> > leg_name_vec_list;
+ for (size_t i = 0; i < fss.length(); i++) {
+ std::vector<coordinates> fs_vec;
+ std::vector<std::string> leg_name_vec;
+ for (size_t j = 0; j < fss[i].fs.length(); j++) {
+ std::string leg(fss[i].fs[j].leg);
+ if (std::find(leg_names.begin(), leg_names.end(), leg) != leg_names.end()) {
+ memcpy(tmpfs.pos.data(), fss[i].fs[j].pos, sizeof(double)*3);
+ tmpfs.rot = (Eigen::Quaternion<double>(fss[i].fs[j].rot[0], fss[i].fs[j].rot[1], fss[i].fs[j].rot[2], fss[i].fs[j].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
+ initial_input_step.worldcoords.transformation(fstrans, tmpfs);
+ tmpfs = initial_support_step.worldcoords;
+ tmpfs.transform(fstrans);
+ } else {
+ std::cerr << "[" << m_profile.instance_name << "] No such target : " << leg << std::endl;
+ return false;
+ }
+ leg_name_vec.push_back(leg);
+ fs_vec.push_back(tmpfs);
+ }
+ leg_name_vec_list.push_back(leg_name_vec);
+ fs_vec_list.push_back(fs_vec);
  }
- if (!gg->get_footstep_coords_by_index(initial_support_coords, overwrite_fs_idx-1)) {
- std::cerr << "[" << m_profile.instance_name << "] Invalid overwrite index = " << overwrite_fs_idx << std::endl;
+ if (spss.length() != fs_vec_list.size()) {
+ std::cerr << "[" << m_profile.instance_name << "] StepParam length " << spss.length () << " != Footstep length " << fs_vec_list.size() << std::endl;
  return false;
  }
+ std::cerr << "[" << m_profile.instance_name << "] print footsteps " << std::endl;
+ std::vector< std::vector<step_node> > fnsl;
+ for (size_t i = 0; i < fs_vec_list.size(); i++) {
+ if (!(gg_is_walking && i == 0)) { // If initial footstep, e.g., not walking, pass user-defined footstep list. If walking, pass cdr footsteps in order to neglect initial double support leg.
+ std::vector<step_node> tmp_fns;
+ for (size_t j = 0; j < fs_vec_list.at(j).size(); j++) {
+ tmp_fns.push_back(step_node(leg_name_vec_list[i][j], fs_vec_list[i][j], spss[i].sps[j].step_height, spss[i].sps[j].step_time, spss[i].sps[j].toe_angle, spss[i].sps[j].heel_angle));
+ }
+ fnsl.push_back(tmp_fns);
+ }
+ }
+ if (gg_is_walking) {
+ std::cerr << "[" << m_profile.instance_name << "] Set overwrite footsteps" << std::endl;
+ gg->set_overwrite_foot_steps_list(fnsl);
+ gg->set_overwrite_foot_step_index(overwrite_fs_idx);
+ } else {
+ std::cerr << "[" << m_profile.instance_name << "] Set normal footsteps" << std::endl;
+ gg->set_foot_steps_list(fnsl);
+ startWalking();
+ }
+ return true;
  } else {
- // If walking, snap initial leg to current ABC foot coords.
- initial_support_coords = ikp[std::string(fs[0].leg)].target_end_coords;
- }
- memcpy(initial_input_coords.pos.data(), fs[0].pos, sizeof(double)*3);
- initial_input_coords.rot = (Eigen::Quaternion<double>(fs[0].rot[0], fs[0].rot[1], fs[0].rot[2], fs[0].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
-
- // Get footsteps
- std::vector<coordinates> fs_vec;
- std::vector<std::string> leg_name_vec;
- for (size_t i = 0; i < fs.length(); i++) {
- std::string leg(fs[i].leg);
- if (leg == "rleg" || leg == "lleg") {
- memcpy(tmpfs.pos.data(), fs[i].pos, sizeof(double)*3);
- tmpfs.rot = (Eigen::Quaternion<double>(fs[i].rot[0], fs[i].rot[1], fs[i].rot[2], fs[i].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
- initial_input_coords.transformation(fstrans, tmpfs);
- tmpfs = initial_support_coords;
- tmpfs.transform(fstrans);
- leg_name_vec.push_back(leg);
- fs_vec.push_back(tmpfs);
- } else {
- std::cerr << "[" << m_profile.instance_name << "] No such target : " << leg << std::endl;
- return false;
- }
- }
- if (sps.length() != fs_vec.size()) {
- std::cerr << "[" << m_profile.instance_name << "] StepParam length " << sps.length () << " != Footstep length " << fs_vec.size() << std::endl;
+ std::cerr << "[" << m_profile.instance_name << "] Cannot setFootSteps while walking." << std::endl;
  return false;
  }
- std::cerr << "[" << m_profile.instance_name << "] print footsteps " << std::endl;
- std::vector< std::vector<step_node> > fnsl;
- for (size_t i = 0; i < fs_vec.size(); i++) {
- if (!(gg_is_walking && i == 0)) // If initial footstep, e.g., not walking, pass user-defined footstep list. If walking, pass cdr footsteps in order to neglect initial double support leg.
- fnsl.push_back(boost::assign::list_of(step_node(leg_name_vec[i], fs_vec[i], sps[i].step_height, sps[i].step_time, sps[i].toe_angle, sps[i].heel_angle)));
- }
- if (gg_is_walking) {
- std::cerr << "[" << m_profile.instance_name << "] Set overwrite footsteps" << std::endl;
- gg->set_overwrite_foot_steps_list(fnsl);
- gg->set_overwrite_foot_step_index(overwrite_fs_idx);
- } else {
- std::cerr << "[" << m_profile.instance_name << "] Set normal footsteps" << std::endl;
- gg->set_foot_steps_list(fnsl);
- startWalking();
- }
- return true;
- } else {
- std::cerr << "[" << m_profile.instance_name << "] Cannot setFootSteps while walking." << std::endl;
- return false;
- }
 }
 
 void AutoBalancer::waitFootSteps()

rtc/AutoBalancer/AutoBalancer.h

@@ -94,7 +94,9 @@ class AutoBalancer
  bool goStop();
  bool emergencyStop ();
  bool setFootSteps(const OpenHRP::AutoBalancerService::FootstepSequence& fs, CORBA::Long overwrite_fs_idx);
+ bool setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx);
  bool setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepSequence& fs, const OpenHRP::AutoBalancerService::StepParamSequence& sps, CORBA::Long overwrite_fs_idx);
+ bool setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx);
  void waitFootSteps();
  void waitFootStepsEarly(const double tm);
  bool startAutoBalancer(const ::OpenHRP::AutoBalancerService::StrSequence& limbs);

rtc/AutoBalancer/AutoBalancerService_impl.cpp

@@ -29,14 +29,14 @@ CORBA::Boolean AutoBalancerService_impl::emergencyStop()
  return m_autobalancer->emergencyStop();
 };
 
-CORBA::Boolean AutoBalancerService_impl::setFootSteps(const OpenHRP::AutoBalancerService::FootstepSequence& fs, CORBA::Long overwrite_fs_idx)
+CORBA::Boolean AutoBalancerService_impl::setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx)
 {
- return m_autobalancer->setFootSteps(fs, overwrite_fs_idx);
+ return m_autobalancer->setFootSteps(fss, overwrite_fs_idx);
 }
 
-CORBA::Boolean AutoBalancerService_impl::setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepSequence& fs, const OpenHRP::AutoBalancerService::StepParamSequence& sps, CORBA::Long overwrite_fs_idx)
+CORBA::Boolean AutoBalancerService_impl::setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx)
 {
- return m_autobalancer->setFootStepsWithParam(fs, sps, overwrite_fs_idx);
+ return m_autobalancer->setFootStepsWithParam(fss, spss, overwrite_fs_idx);
 }
 
 void AutoBalancerService_impl::waitFootSteps()

rtc/AutoBalancer/AutoBalancerService_impl.h

@@ -19,8 +19,8 @@ class AutoBalancerService_impl
  CORBA::Boolean goVelocity( CORBA::Double vx, CORBA::Double vy, CORBA::Double vth);
  CORBA::Boolean goStop();
  CORBA::Boolean emergencyStop();
- CORBA::Boolean setFootSteps(const OpenHRP::AutoBalancerService::FootstepSequence& fs, CORBA::Long overwrite_fs_idx);
- CORBA::Boolean setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepSequence& fs, const OpenHRP::AutoBalancerService::StepParamSequence& sps, CORBA::Long overwrite_fs_idx);
+ CORBA::Boolean setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx);
+ CORBA::Boolean setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx);
  void waitFootSteps();
  void waitFootStepsEarly(CORBA::Double tm);
  CORBA::Boolean startAutoBalancer(const OpenHRP::AutoBalancerService::StrSequence& limbs);

rtc/AutoBalancer/GaitGenerator.h

@@ -947,10 +947,10 @@ namespace rats
  return false;
  }
  };
- bool get_footstep_coords_by_index (coordinates& cs, const size_t idx)
+ bool get_footstep_nodes_by_index (std::vector<step_node>& csl, const size_t idx)
  {
  if (footstep_nodes_list.size()-1 >= idx) {
- cs = footstep_nodes_list[idx].front().worldcoords;
+ csl = footstep_nodes_list.at(idx);
  return true;
  } else {
  return false;