Merge remote-tracking branch 'origin/master' into add-baseRpyCurrent-…

…port
snozawa · Apr 25, 2015 · 3f8b9bd · 3f8b9bd
2 parents e7a0b5e + bdc08a7
commit 3f8b9bd
Show file tree

Hide file tree

Showing 9 changed files with 214 additions and 230 deletions.
diff --git a/idl/AutoBalancerService.idl b/idl/AutoBalancerService.idl
@@ -27,11 +27,22 @@ module OpenHRP
  string leg;
  };
 
+ /**
+ * @struct StepParam
+ * @brief Step parameter for one step
+ */
+ struct StepParam
+ {
+ /// Step height [m]
+ double step_height;
+ };
+
  /**
  * @struct FootstepSequence
  * @brief Sequence of foot step.
  */
  typedef sequence<Footstep> FootstepSequence;
+ typedef sequence<StepParam> StepParamSequence;
 
  /**
  * @enum SupportLegState
@@ -178,6 +189,13 @@ module OpenHRP
  */
  boolean setFootSteps(in FootstepSequence fs);
 
+ /**
+ * @brief Set footsteps. Returns without waiting for whole steps to be executed.
+ * @param fs is sequence of FootStepWithParam structure.
+ * @return true if set successfully, false otherwise
+ */
+ boolean setFootStepsWithParam(in FootstepSequence fs, in StepParamSequence sps);
+
  /**
  * @brief Wait for whole footsteps are executed.
  * @param

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -922,6 +922,14 @@ bool AutoBalancer::goStop ()
 }
 
 bool AutoBalancer::setFootSteps(const OpenHRP::AutoBalancerService::FootstepSequence& fs)
+{
+ OpenHRP::AutoBalancerService::StepParamSequence sps;
+ sps.length(fs.length());
+ for (size_t i = 0; i < sps.length(); i++) sps[i].step_height = gg->get_default_step_height();
+ setFootStepsWithParam(fs, sps);
+}
+
+bool AutoBalancer::setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepSequence& fs, const OpenHRP::AutoBalancerService::StepParamSequence& sps)
 {
  if (!gg_is_walking) {
  std::cerr << "[" << m_profile.instance_name << "] setFootSteps" << std::endl;
@@ -957,7 +965,7 @@ bool AutoBalancer::setFootSteps(const OpenHRP::AutoBalancerService::FootstepSequ
  std::cerr << "[" << m_profile.instance_name << "] print footsteps " << std::endl;
  gg->clear_footstep_node_list();
  for (size_t i = 0; i < fs_vec.size(); i++) {
- gg->append_footstep_node(leg_name_vec[i], fs_vec[i]);
+ gg->append_footstep_node(leg_name_vec[i], fs_vec[i], sps[i].step_height);
  }
  gg->append_finalize_footstep();
  gg->print_footstep_list();

diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
@@ -93,6 +93,7 @@ class AutoBalancer
  bool goVelocity(const double& vx, const double& vy, const double& vth);
  bool goStop();
  bool setFootSteps(const OpenHRP::AutoBalancerService::FootstepSequence& fs);
+ bool setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepSequence& fs, const OpenHRP::AutoBalancerService::StepParamSequence& sps);
  void waitFootSteps();
  bool startAutoBalancer(const ::OpenHRP::AutoBalancerService::StrSequence& limbs);
  bool stopAutoBalancer();

diff --git a/rtc/AutoBalancer/AutoBalancerService_impl.cpp b/rtc/AutoBalancer/AutoBalancerService_impl.cpp
@@ -29,6 +29,11 @@ CORBA::Boolean AutoBalancerService_impl::setFootSteps(const OpenHRP::AutoBalance
  return m_autobalancer->setFootSteps(fs);
 }
 
+CORBA::Boolean AutoBalancerService_impl::setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepSequence& fs, const OpenHRP::AutoBalancerService::StepParamSequence& sps)
+{
+ return m_autobalancer->setFootStepsWithParam(fs, sps);
+}
+
 void AutoBalancerService_impl::waitFootSteps()
 {
  return m_autobalancer->waitFootSteps();

diff --git a/rtc/AutoBalancer/AutoBalancerService_impl.h b/rtc/AutoBalancer/AutoBalancerService_impl.h
@@ -19,6 +19,7 @@ class AutoBalancerService_impl
  CORBA::Boolean goVelocity( CORBA::Double vx, CORBA::Double vy, CORBA::Double vth);
  CORBA::Boolean goStop();
  CORBA::Boolean setFootSteps(const OpenHRP::AutoBalancerService::FootstepSequence& fs);
+ CORBA::Boolean setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepSequence& fs, const OpenHRP::AutoBalancerService::StepParamSequence& sps);
  void waitFootSteps();
  CORBA::Boolean startAutoBalancer(const OpenHRP::AutoBalancerService::StrSequence& limbs);
  CORBA::Boolean stopAutoBalancer();

diff --git a/rtc/AutoBalancer/GaitGenerator.cpp b/rtc/AutoBalancer/GaitGenerator.cpp
@@ -237,7 +237,7 @@ namespace rats
  }
  if (gp_index < fnl.size() - 1) {
  if (force_height_zero) current_step_height = 0.0;
- else current_step_height = default_step_height;
+ else current_step_height = fnl[gp_index].step_height;
  } else {
  current_step_height = 0.0;
  }
@@ -371,9 +371,9 @@ namespace rats
  const coordinates& _support_leg_coords)
  {
  leg_type _swing_leg = (tmp_swing_leg == "rleg") ? WC_RLEG : WC_LLEG;
- step_node sn0((_swing_leg == WC_RLEG) ? WC_LLEG : WC_RLEG, _support_leg_coords);
+ step_node sn0((_swing_leg == WC_RLEG) ? WC_LLEG : WC_RLEG, _support_leg_coords, lcg.get_default_step_height());
  footstep_node_list.push_back(sn0);
- step_node sn1(_swing_leg, _support_leg_coords);
+ step_node sn1(_swing_leg, _support_leg_coords, lcg.get_default_step_height());
  hrp::Vector3 trs(2.0 * footstep_param.leg_default_translate_pos[(_swing_leg == WC_RLEG) ? 0 : 1] + hrp::Vector3(goal_x, goal_y, goal_z));
  sn1.worldcoords.pos += sn1.worldcoords.rot * trs;
  sn1.worldcoords.rotate(deg2rad(goal_theta), hrp::Vector3(0,0,1));
@@ -475,7 +475,7 @@ namespace rats
  if ( footstep_node_list.size() - 1 >= idx + i) /* if footstep_node_list[idx] and footstep_node_list[idx+1] exists */
  footstep_node_list[idx + i].worldcoords = cv[i];
  else
- footstep_node_list.push_back(step_node(footstep_node_list[lcg.get_gp_index()-1 + i].l_r, cv[i]));
+ footstep_node_list.push_back(step_node(footstep_node_list[lcg.get_gp_index()-1 + i].l_r, cv[i], lcg.get_default_step_height()));
  }
 
  /* remove steps after newly added steps */

diff --git a/rtc/AutoBalancer/GaitGenerator.h b/rtc/AutoBalancer/GaitGenerator.h
@@ -24,10 +24,11 @@ namespace rats
  {
  leg_type l_r;
  coordinates worldcoords;
- step_node (const leg_type _l_r, const coordinates& _worldcoords)
- : l_r(_l_r), worldcoords(_worldcoords) {};
- step_node (const std::string& _l_r, const coordinates& _worldcoords)
- : l_r((_l_r == "rleg") ? WC_RLEG : WC_LLEG), worldcoords(_worldcoords) {};
+ double step_height;
+ step_node (const leg_type _l_r, const coordinates& _worldcoords, const double _step_height)
+ : l_r(_l_r), worldcoords(_worldcoords), step_height(_step_height) {};
+ step_node (const std::string& _l_r, const coordinates& _worldcoords, const double _step_height)
+ : l_r((_l_r == "rleg") ? WC_RLEG : WC_LLEG), worldcoords(_worldcoords), step_height(_step_height) {};
  };
  friend std::ostream &operator<<(std::ostream &os, const step_node &sn)
  {
@@ -37,6 +38,7 @@ namespace rats
  os << (sn.worldcoords.pos).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << std::endl;
  os << " rot =" << std::endl;
  os << (sn.worldcoords.rot).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", " [", "]")) << std::endl;
+ os << " step_height = " << sn.step_height << "[m]" << std::endl;
  return os;
  };
 
@@ -85,7 +87,11 @@ namespace rats
 #endif
  refzmp_generator()
  : refzmp_cur_list(), default_zmp_offsets(),
- fs_index(0), refzmp_index(0), refzmp_count(0) {};
+ fs_index(0), refzmp_index(0), refzmp_count(0)
+ {
+ default_zmp_offsets.push_back(hrp::Vector3::Zero());
+ default_zmp_offsets.push_back(hrp::Vector3::Zero());
+ };
  ~refzmp_generator() {};
  /* */
  void remove_refzmp_cur_list_over_length (const size_t len)
@@ -443,7 +449,7 @@ namespace rats
  void append_go_pos_step_node (const coordinates& _foot_midcoords,
  const leg_type _l_r)
  {
- step_node sn(_l_r, _foot_midcoords);
+ step_node sn(_l_r, _foot_midcoords, lcg.get_default_step_height());
  sn.worldcoords.pos += sn.worldcoords.rot * footstep_param.leg_default_translate_pos[(_l_r == WC_RLEG) ? 0 : 1];
  footstep_node_list.push_back(sn);
  };
@@ -483,7 +489,11 @@ namespace rats
  bool proc_one_tick ();
  void append_footstep_node (const std::string& _leg, const coordinates& _fs)
  {
- footstep_node_list.push_back(step_node((_leg == "rleg") ? WC_RLEG : WC_LLEG, _fs));
+ footstep_node_list.push_back(step_node((_leg == "rleg") ? WC_RLEG : WC_LLEG, _fs, lcg.get_default_step_height()));
+ };
+ void append_footstep_node (const std::string& _leg, const coordinates& _fs, const double _step_height)
+ {
+ footstep_node_list.push_back(step_node((_leg == "rleg") ? WC_RLEG : WC_LLEG, _fs, _step_height));
  };
  void clear_footstep_node_list () { footstep_node_list.clear(); };
  void go_pos_param_2_footstep_list (const double goal_x, const double goal_y, const double goal_theta, /* [mm] [mm] [deg] */