merge origin/master

idl/AutoBalancerService.idl

@@ -183,6 +183,8 @@ module OpenHRP
  boolean use_toe_joint;
  /// Use toe heel zmp transition. If true, zmp moves among default position, toe position (described by toe_zmp_offset_x), and heel position (described by heel_zmp_offset_x).
  boolean use_toe_heel_transition;
+ /// ZMP weight of RLEG, LLEG, RARM and LARM
+ sequence<double, 4> zmp_weight_map;
  /// Foot position offset[m] (rleg and lleg)
  sequence<DblSequence3> leg_default_translate_pos;
  /// Number of optional finalize footsteps in goPos
@@ -196,7 +198,7 @@ module OpenHRP
  struct AutoBalancerParam
  {
  /// ZMP offset vectors[m] for rleg and lleg (<-please set by this order)
- sequence<DblSequence3, 2> default_zmp_offsets;
+ sequence<DblSequence3> default_zmp_offsets;
  double move_base_gain;
  ControllerMode controller_mode;
  boolean graspless_manip_mode;

idl/StabilizerService.idl

@@ -76,15 +76,15 @@ module OpenHRP
  DblArray2 eefm_zmp_delay_time_const;
  /// Auxiliary input for ZMP position [m] (x,y). This is used for delay model identification
  DblArray2 eefm_ref_zmp_aux;
- /// Foot rotation damping gain [Nm/(rad/s)].
- double eefm_rot_damping_gain;
- /// Foot rotation damping time constant [s].
- double eefm_rot_time_const;
- /// Foot position damping gain [N/(m/s)].
- DblArray3 eefm_pos_damping_gain;
- /// Foot position damping time constant for double support phase [s].
- DblArray3 eefm_pos_time_const_support;
- /// Foot position damping time constant for single support phase [s].
+ /// Sequence of all end-effector rotation damping gain [Nm/(rad/s)] (r,p,y).
+ sequence<sequence<double, 3> > eefm_rot_damping_gain;
+ /// Sequence of all end-effector rotation damping time constant [s] (r,p,y).
+ sequence<sequence<double, 3> > eefm_rot_time_const;
+ /// Sequence of all end-effector position damping gain [N/(m/s)] (x,y,z).
+ sequence<sequence<double, 3> > eefm_pos_damping_gain;
+ /// Sequence of all end-effector position damping time constant for double support phase [s] (x,y,z).
+ sequence<sequence<double, 3> > eefm_pos_time_const_support;
+ /// End-effector position damping time constant for single support phase [s].
  double eefm_pos_time_const_swing;
  /// Transition time for single=>double support phase gain interpolation [s].
  double eefm_pos_transition_time;

launch/samplerobot.launch

@@ -21,6 +21,8 @@
  <arg unless="$(arg TORQUE_CONTROL)" name="JOINT_CONTROLLER" default="HGcontroller" />
  <arg if="$(arg TORQUE_CONTROL)" name="JOINT_CONTROLLER" default="PDcontroller" />
  <env name="ROS_HOME" value="$(find hrpsys)/samples/SampleRobot" />
+ <env if="$(arg TORQUE_CONTROL)" name="BUSH_CUSTOMIZER_CONFIG_PATH" value="$(find openhrp3)/../OpenHRP-3.1/customizer/sample1_bush_customizer_param.conf" />
+ <env unless="$(arg TORQUE_CONTROL)" name="BUSH_CUSTOMIZER_CONFIG_PATH" value="" />
  <!-- END:common setting -->
 
  <arg unless="$(arg GUI)" name="hrpsys_gui_args" value="-endless -realtime -nodisplay" />

rtc/AutoBalancer/AutoBalancer.h

@@ -184,6 +184,7 @@ class AutoBalancer
  rats::coordinates target_end_coords;
  hrp::Link* target_link;
  hrp::JointPathExPtr manip;
+ size_t pos_ik_error_count, rot_ik_error_count;
  bool is_active;
  };
  void getCurrentParameters();
@@ -224,7 +225,7 @@ class AutoBalancer
  coil::Mutex m_mutex;
 
  double transition_interpolator_ratio, transition_time, zmp_transition_time, adjust_footstep_transition_time;
- interpolator *zmp_interpolator;
+ interpolator *zmp_offset_interpolator;
  interpolator *transition_interpolator;
  interpolator *adjust_footstep_interpolator;
  hrp::Vector3 input_zmp, input_basePos;
@@ -237,7 +238,7 @@ class AutoBalancer
 
  unsigned int m_debugLevel;
  bool is_legged_robot, is_stop_mode, has_ik_failed;
- int loop;
+ int loop, ik_error_debug_print_freq;
  bool graspless_manip_mode;
  std::string graspless_manip_arm;
  hrp::Vector3 graspless_manip_p_gain;

rtc/AutoBalancer/AutoBalancerService_impl.cpp

@@ -69,6 +69,7 @@ CORBA::Boolean AutoBalancerService_impl::getGaitGeneratorParam(OpenHRP::AutoBala
  i_param = new OpenHRP::AutoBalancerService::GaitGeneratorParam();
  i_param->stride_parameter.length(4);
  i_param->toe_heel_phase_ratio.length(7);
+ i_param->zmp_weight_map.length(4);
  return m_autobalancer->getGaitGeneratorParam(*i_param);
 };
 
@@ -80,9 +81,6 @@ CORBA::Boolean AutoBalancerService_impl::setAutoBalancerParam(const OpenHRP::Aut
 CORBA::Boolean AutoBalancerService_impl::getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalancerParam_out i_param)
 {
  i_param = new OpenHRP::AutoBalancerService::AutoBalancerParam();
- i_param->default_zmp_offsets.length(2);
- for (size_t i = 0; i < 2; i++)
- i_param->default_zmp_offsets[i].length(3);
  return m_autobalancer->getAutoBalancerParam(*i_param);
 };
 

rtc/AutoBalancer/GaitGenerator.cpp

@@ -61,14 +61,18 @@ namespace rats
  hrp::Vector3 ret_zmp;
  hrp::Vector3 tmp_zero = hrp::Vector3::Zero();
  std::vector<hrp::Vector3> foot_x_axises;
- for (std::vector<step_node>::const_iterator it = _support_leg_steps.begin(); it != _support_leg_steps.end(); it++)
- dzl.push_back(it->worldcoords.rot * default_zmp_offsets[it->l_r] + it->worldcoords.pos);
+ double sum_of_weight = 0.0;
+ for (std::vector<step_node>::const_iterator it = _support_leg_steps.begin(); it != _support_leg_steps.end(); it++) {
+ dzl.push_back((it->worldcoords.rot * default_zmp_offsets[it->l_r] + it->worldcoords.pos) * zmp_weight_map[it->l_r]);
+ sum_of_weight += zmp_weight_map[it->l_r];
+ }
  for (std::vector<step_node>::const_iterator it = _swing_leg_steps.begin(); it != _swing_leg_steps.end(); it++) {
- dzl.push_back(it->worldcoords.rot * default_zmp_offsets[it->l_r] + it->worldcoords.pos);
+ dzl.push_back((it->worldcoords.rot * default_zmp_offsets[it->l_r] + it->worldcoords.pos) * zmp_weight_map[it->l_r]);
+ sum_of_weight += zmp_weight_map[it->l_r];
  foot_x_axises.push_back( hrp::Vector3(it->worldcoords.rot * hrp::Vector3::UnitX()) );
  }
  foot_x_axises_list.push_back(foot_x_axises);
- rzmp = std::accumulate(dzl.begin(), dzl.end(), tmp_zero) / dzl.size();
+ rzmp = std::accumulate(dzl.begin(), dzl.end(), tmp_zero) / sum_of_weight;
  refzmp_cur_list.push_back( rzmp );
  std::vector<leg_type> swing_leg_types;
  for (size_t i = 0; i < fns.size(); i++) {
@@ -86,12 +90,14 @@ namespace rats
  hrp::Vector3 rzmp, tmp_zero=hrp::Vector3::Zero();
  std::vector<hrp::Vector3> dzl;
  std::vector<hrp::Vector3> foot_x_axises;
+ double sum_of_weight = 0.0;
 
  for (std::vector<step_node>::const_iterator it = _support_leg_steps.begin(); it != _support_leg_steps.end(); it++) {
- dzl.push_back(it->worldcoords.rot * default_zmp_offsets[it->l_r] + it->worldcoords.pos);
- foot_x_axises.push_back( hrp::Vector3(it->worldcoords.rot * hrp::Vector3::UnitX()) );
+ dzl.push_back((it->worldcoords.rot * default_zmp_offsets[it->l_r] + it->worldcoords.pos) * zmp_weight_map[it->l_r]);
+ sum_of_weight += zmp_weight_map[it->l_r];
+ foot_x_axises.push_back( hrp::Vector3(it->worldcoords.rot * hrp::Vector3::UnitX()) );
  }
- rzmp = std::accumulate(dzl.begin(), dzl.end(), tmp_zero) / dzl.size();
+ rzmp = std::accumulate(dzl.begin(), dzl.end(), tmp_zero) / sum_of_weight;
  refzmp_cur_list.push_back( rzmp );
  foot_x_axises_list.push_back(foot_x_axises);
  std::vector<leg_type> swing_leg_types;
@@ -469,8 +475,8 @@ namespace rats
  rg.push_refzmp_from_footstep_nodes_for_single(footstep_nodes_list.at(i), lcg.get_support_leg_steps_idx(i));
  }
  rg.push_refzmp_from_footstep_nodes_for_dual(footstep_nodes_list.back(),
- lcg.get_swing_leg_dst_steps_idx(footstep_nodes_list.size()-1),
- lcg.get_support_leg_steps_idx(footstep_nodes_list.size()-1));
+ lcg.get_support_leg_steps_idx(footstep_nodes_list.size()-1),
+ lcg.get_swing_leg_dst_steps_idx(footstep_nodes_list.size()-1));
  emergency_flg = IDLING;
  };
 

rtc/AutoBalancer/GaitGenerator.h

@@ -8,6 +8,7 @@
 #include <queue>
 #include <boost/assign.hpp>
 #include <boost/lambda/lambda.hpp>
+#include <boost/shared_ptr.hpp>
 
 namespace rats
 {
@@ -204,6 +205,7 @@ namespace rats
  public:
 #endif
  std::vector<hrp::Vector3> refzmp_cur_list;
+ std::map<leg_type, double> zmp_weight_map;
  std::vector< std::vector<hrp::Vector3> > foot_x_axises_list; // Swing foot x axis list according to refzmp_cur_list
  std::vector< std::vector<leg_type> > swing_leg_types_list; // Swing leg list according to refzmp_cur_list
  std::vector<size_t> step_count_list; // Swing leg list according to refzmp_cur_list
@@ -213,6 +215,7 @@ namespace rats
  double dt;
  toe_heel_phase_counter* thp_ptr;
  bool use_toe_heel_transition;
+ boost::shared_ptr<interpolator> zmp_weight_interpolator;
  void calc_current_refzmp (hrp::Vector3& ret, std::vector<hrp::Vector3>& swing_foot_zmp_offsets, const double default_double_support_ratio, const double default_double_support_static_ratio) const;
  const bool is_start_double_support_phase () const { return refzmp_index == 0; };
  const bool is_second_phase () const { return refzmp_index == 1; };
@@ -231,6 +234,10 @@ namespace rats
  default_zmp_offsets.push_back(hrp::Vector3::Zero());
  default_zmp_offsets.push_back(hrp::Vector3::Zero());
  default_zmp_offsets.push_back(hrp::Vector3::Zero());
+ double zmp_weight_initial_value[4] = {1.0, 1.0, 0.1, 0.1};
+ zmp_weight_map = boost::assign::map_list_of<leg_type, double>(RLEG, zmp_weight_initial_value[0])(LLEG, zmp_weight_initial_value[1])(RARM, zmp_weight_initial_value[2])(LARM, zmp_weight_initial_value[3]);
+ zmp_weight_interpolator = boost::shared_ptr<interpolator>(new interpolator(4, dt));
+ zmp_weight_interpolator->set(zmp_weight_initial_value); /* set initial value */
  };
  ~refzmp_generator()
  {
@@ -265,6 +272,17 @@ namespace rats
  void set_toe_zmp_offset_x (const double _off) { toe_zmp_offset_x = _off; };
  void set_heel_zmp_offset_x (const double _off) { heel_zmp_offset_x = _off; };
  void set_use_toe_heel_transition (const double _u) { use_toe_heel_transition = _u; };
+ void set_zmp_weight_map (const std::map<leg_type, double> _map) {
+ double zmp_weight_array[4] = {_map.find(RLEG)->second, _map.find(LLEG)->second, _map.find(RARM)->second, _map.find(LARM)->second};
+ if (zmp_weight_interpolator->isEmpty()) {
+ zmp_weight_interpolator->clear();
+ double zmp_weight_initial_value[4] = {zmp_weight_map[RLEG], zmp_weight_map[LLEG], zmp_weight_map[RARM], zmp_weight_map[LARM]};
+ zmp_weight_interpolator->set(zmp_weight_initial_value);
+ zmp_weight_interpolator->go(zmp_weight_array, 2.0, true);
+ } else {
+ std::cerr << "zmp_weight_map cannot be set because interpolating." << std::endl;
+ }
+ };
  // getter
  bool get_current_refzmp (hrp::Vector3& rzmp, std::vector<hrp::Vector3>& swing_foot_zmp_offsets, const double default_double_support_ratio, const double default_double_support_static_ratio) const
  {
@@ -276,6 +294,14 @@ namespace rats
  double get_toe_zmp_offset_x () const { return toe_zmp_offset_x; };
  double get_heel_zmp_offset_x () const { return heel_zmp_offset_x; };
  bool get_use_toe_heel_transition () const { return use_toe_heel_transition; };
+ const std::map<leg_type, double> get_zmp_weight_map () const { return zmp_weight_map; };
+ void proc_zmp_weight_map_interpolation () {
+ if (!zmp_weight_interpolator->isEmpty()) {
+ double zmp_weight_output[4];
+ zmp_weight_interpolator->get(zmp_weight_output, true);
+ zmp_weight_map = boost::assign::map_list_of<leg_type, double>(RLEG, zmp_weight_output[0])(LLEG, zmp_weight_output[1])(RARM, zmp_weight_output[2])(LARM, zmp_weight_output[3]);
+ }
+ };
  };
 
  class delay_hoffarbib_trajectory_generator
@@ -887,6 +913,7 @@ namespace rats
  void set_toe_zmp_offset_x (const double _off) { rg.set_toe_zmp_offset_x(_off); };
  void set_heel_zmp_offset_x (const double _off) { rg.set_heel_zmp_offset_x(_off); };
  void set_use_toe_heel_transition (const double _u) { rg.set_use_toe_heel_transition(_u); };
+ void set_zmp_weight_map (const std::map<leg_type, double> _map) { rg.set_zmp_weight_map(_map); };
  void set_default_step_height(const double _tmp) { lcg.set_default_step_height(_tmp); };
  void set_default_top_ratio(const double _tmp) { lcg.set_default_top_ratio(_tmp); };
  void set_velocity_param (const double vel_x, const double vel_y, const double vel_theta) /* [mm/s] [mm/s] [deg/s] */
@@ -996,6 +1023,8 @@ namespace rats
  double get_toe_zmp_offset_x () const { return rg.get_toe_zmp_offset_x(); };
  double get_heel_zmp_offset_x () const { return rg.get_heel_zmp_offset_x(); };
  bool get_use_toe_heel_transition () const { return rg.get_use_toe_heel_transition(); };
+ const std::map<leg_type, double> get_zmp_weight_map () const { return rg.get_zmp_weight_map(); };
+ void proc_zmp_weight_map_interpolation () { return rg.proc_zmp_weight_map_interpolation(); };
  std::vector<std::string> get_footstep_front_leg_names () const {
  std::vector<leg_type> lts;
  for (size_t i = 0; i < footstep_nodes_list[0].size(); i++) {