[idl/AutoBalancerService.idl, AutoBalancer.cpp, AutoBalancerService_i…

…mpl.cpp] set the number of default_zmp_offsets according to the number of end-effectors

idl/AutoBalancerService.idl

@@ -198,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;

rtc/AutoBalancer/AutoBalancer.cpp

@@ -207,7 +207,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
  for (size_t i = 0; i < num; i++) m_controlSwingSupportTime.data[i] = 0.0;
  }
 
- zmp_interpolator = new interpolator(6, m_dt);
+ zmp_interpolator = new interpolator(ikp.size()*3, m_dt);
  zmp_transition_time = 1.0;
  transition_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
  transition_interpolator_ratio = 1.0;
@@ -222,7 +222,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
  double stride_bwd_x_limit = 0.05;
  std::cerr << "[" << m_profile.instance_name << "] abc_stride_parameter : " << stride_fwd_x_limit << "[m], " << stride_y_limit << "[m], " << stride_th_limit << "[deg], " << stride_bwd_x_limit << "[m]" << std::endl;
  if (default_zmp_offsets.size() == 0) {
- for (size_t i = 0; i < leg_pos.size(); i++) default_zmp_offsets.push_back(hrp::Vector3::Zero());
+ for (size_t i = 0; i < ikp.size(); i++) default_zmp_offsets.push_back(hrp::Vector3::Zero());
  }
  if (leg_offset_str.size() > 0) {
  gg = ggPtr(new rats::gait_generator(m_dt, leg_pos, leg_names, stride_fwd_x_limit/*[m]*/, stride_y_limit/*[m]*/, stride_th_limit/*[deg]*/, stride_bwd_x_limit/*[m]*/));
@@ -540,15 +540,19 @@ void AutoBalancer::getTargetParameters()
  if (control_mode != MODE_IDLE) {
  coordinates tmp_fix_coords;
  if (!zmp_interpolator->isEmpty()) {
- double default_zmp_offsets_output[leg_names.size() * 3];
+ double *default_zmp_offsets_output = new double[ikp.size()*3];
  zmp_interpolator->get(default_zmp_offsets_output, true);
- for (size_t i = 0; i < leg_names.size(); i++)
+ for (size_t i = 0; i < ikp.size(); i++)
  for (size_t j = 0; j < 3; j++)
  default_zmp_offsets[i](j) = default_zmp_offsets_output[i*3+j];
+ delete[] default_zmp_offsets_output;
  if (DEBUGP) {
  std::cerr << "[" << m_profile.instance_name << "] default_zmp_offsets (interpolated)" << std::endl;
- for (size_t i = 0; i < leg_names.size(); i++)
- std::cerr << "[" << m_profile.instance_name << "] " << leg_names[i] << " = " << default_zmp_offsets[i].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl;
+ std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+ for (size_t i = 0; i < leg_names.size(); i++) {
+ 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 == leg_names[i]));
+ std::cerr << "[" << m_profile.instance_name << "] " << leg_names[i] << " = " << default_zmp_offsets[dst->first].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl;
+ }
  }
  }
  if ( gg_is_walking ) {
@@ -1259,9 +1263,9 @@ bool AutoBalancer::getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGener
 bool AutoBalancer::setAutoBalancerParam(const OpenHRP::AutoBalancerService::AutoBalancerParam& i_param)
 {
  std::cerr << "[" << m_profile.instance_name << "] setAutoBalancerParam" << std::endl;
- double default_zmp_offsets_array[6];
+ double *default_zmp_offsets_array = new double[ikp.size()*3];
  move_base_gain = i_param.move_base_gain;
- for (size_t i = 0; i < 2; i++)
+ for (size_t i = 0; i < ikp.size(); i++)
  for (size_t j = 0; j < 3; j++)
  default_zmp_offsets_array[i*3+j] = i_param.default_zmp_offsets[i][j];
  zmp_transition_time = i_param.zmp_transition_time;
@@ -1288,9 +1292,12 @@ bool AutoBalancer::setAutoBalancerParam(const OpenHRP::AutoBalancerService::Auto
  leg_names.push_back(std::string(i_param.leg_names[i]));
  }
  std::cerr << "[" << m_profile.instance_name << "] move_base_gain = " << move_base_gain << std::endl;
- std::cerr << "[" << m_profile.instance_name << "] default_zmp_offsets = "
- << default_zmp_offsets_array[0] << " " << default_zmp_offsets_array[1] << " " << default_zmp_offsets_array[2] << " "
- << default_zmp_offsets_array[3] << " " << default_zmp_offsets_array[4] << " " << default_zmp_offsets_array[5] << std::endl;
+ std::cerr << "[" << m_profile.instance_name << "] default_zmp_offsets = ";
+ for (size_t i = 0; i < ikp.size() * 3; i++) {
+ std::cerr << default_zmp_offsets_array[i] << " ";
+ }
+ std::cerr << std::endl;
+ delete[] default_zmp_offsets_array;
  std::cerr << "[" << m_profile.instance_name << "] graspless_manip_mode = " << graspless_manip_mode << std::endl;
  std::cerr << "[" << m_profile.instance_name << "] graspless_manip_arm = " << graspless_manip_arm << std::endl;
  std::cerr << "[" << m_profile.instance_name << "] graspless_manip_p_gain = " << graspless_manip_p_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << std::endl;
@@ -1304,9 +1311,13 @@ bool AutoBalancer::setAutoBalancerParam(const OpenHRP::AutoBalancerService::Auto
 bool AutoBalancer::getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalancerParam& i_param)
 {
  i_param.move_base_gain = move_base_gain;
- for (size_t i = 0; i < 2; i++)
- for (size_t j = 0; j < 3; j++)
- i_param.default_zmp_offsets[i][j] = default_zmp_offsets[i](j);
+ i_param.default_zmp_offsets.length(ikp.size());
+ for (size_t i = 0; i < ikp.size(); i++) {
+ i_param.default_zmp_offsets[i].length(3);
+ for (size_t j = 0; j < 3; j++) {
+ i_param.default_zmp_offsets[i][j] = default_zmp_offsets[i](j);
+ }
+ }
  switch(control_mode) {
  case MODE_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_IDLE; break;
  case MODE_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_ABC; break;

rtc/AutoBalancer/AutoBalancerService_impl.cpp

@@ -81,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);
 };
 

sample/SampleRobot/samplerobot_auto_balancer.py

@@ -75,15 +75,15 @@ def demoAutoBalancerGetParam():
 def demoAutoBalancerSetParam():
  print >> sys.stderr, "4. setAutoBalancerParam"
  abcp=hcf.abc_svc.getAutoBalancerParam()[1]
- abcp.default_zmp_offsets = [[0.1,0,0], [0.1,0,0]]
+ abcp.default_zmp_offsets = [[0.1,0,0], [0.1,0,0], [0,0,0], [0,0,0]]
  hcf.abc_svc.setAutoBalancerParam(abcp)
  ret=hcf.abc_svc.getAutoBalancerParam()
  if ret[0] and ret[1].default_zmp_offsets == abcp.default_zmp_offsets:
  print >> sys.stderr, " setAutoBalancerParam() => OK"
  print >> sys.stderr, " default_zmp_offsets setting check in start and stop"
  hcf.startAutoBalancer();
  hcf.stopAutoBalancer();
- abcp.default_zmp_offsets = [[0,0,0], [0,0,0]]
+ abcp.default_zmp_offsets = [[0,0,0], [0,0,0], [0,0,0], [0,0,0]]
  hcf.abc_svc.setAutoBalancerParam(abcp)
 
 def demoAutoBalancerTestPoses():
@@ -96,7 +96,7 @@ def demoAutoBalancerTestPoses():
 def demoAutoBalancerStartStopCheck():
  print >> sys.stderr, "6. start stop check"
  abcp=hcf.abc_svc.getAutoBalancerParam()[1]
- abcp.default_zmp_offsets = [[-0.05,0.05,0], [-0.05,0.05,0]]
+ abcp.default_zmp_offsets = [[-0.05,0.05,0], [-0.05,0.05,0], [0,0,0], [0,0,0]]
  hcf.abc_svc.setAutoBalancerParam(abcp)
  hcf.setMaxLogLength(1500)
  for pose in pose_list:
@@ -106,7 +106,7 @@ def demoAutoBalancerStartStopCheck():
  hcf.startAutoBalancer();
  hcf.stopAutoBalancer();
  hcf.saveLog("/tmp/test-samplerobot-abc-startstop-{0}".format(pose_list.index(pose)))
- abcp.default_zmp_offsets = [[0,0,0], [0,0,0]]
+ abcp.default_zmp_offsets = [[0,0,0], [0,0,0], [0,0,0], [0,0,0]]
  hcf.abc_svc.setAutoBalancerParam(abcp)
  hcf.seq_svc.setJointAngles(initial_pose, 1.0)
  hcf.waitInterpolation()