[StabilizerService.idl, Stabilizer.cpp, Stabilizer.h] add eefm_swing_…

…rot_spring_gain / eefm_swing_rot_spring_gain as st param

idl/StabilizerService.idl

@@ -104,6 +104,10 @@ module OpenHRP
  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;
+ /// Sequence of all swing leg end-effector rotation spring gain (r,p,y).
+ sequence<sequence<double, 3> > eefm_swing_rot_spring_gain;
+ /// Sequence of all swing leg end-effector position spring gain (x,y,z).
+ sequence<sequence<double, 3> > eefm_swing_pos_spring_gain;
  /// Sequence of all end-effector position compensation limit [m]
  sequence<double> eefm_pos_compensation_limit;
  /// Sequence of all end-effector rot compensation limit [rad]

rtc/Stabilizer/Stabilizer.cpp

@@ -297,9 +297,11 @@ RTC::ReturnCode_t Stabilizer::onInitialize()
  ikp.eefm_rot_damping_gain = hrp::Vector3(20*5, 20*5, 1e5);
  ikp.eefm_rot_time_const = hrp::Vector3(1.5, 1.5, 1.5);
  ikp.eefm_rot_compensation_limit = deg2rad(10.0);
+ ikp.eefm_swing_rot_spring_gain = hrp::Vector3(0.0, 0.0, 0.0);
  ikp.eefm_pos_damping_gain = hrp::Vector3(3500*10, 3500*10, 3500);
  ikp.eefm_pos_time_const_support = hrp::Vector3(1.5, 1.5, 1.5);
  ikp.eefm_pos_compensation_limit = 0.025;
+ ikp.eefm_swing_pos_spring_gain = hrp::Vector3(0.0, 0.0, 0.0);
  }
  eefm_pos_time_const_swing = 0.08;
  eefm_pos_transition_time = 0.01;
@@ -1507,19 +1509,25 @@ void Stabilizer::getParameter(OpenHRP::StabilizerService::stParam& i_stp)
  i_stp.eefm_pos_time_const_support.length(stikp.size());
  i_stp.eefm_pos_damping_gain.length(stikp.size());
  i_stp.eefm_pos_compensation_limit.length(stikp.size());
+ i_stp.eefm_swing_pos_spring_gain.length(stikp.size());
  i_stp.eefm_rot_time_const.length(stikp.size());
  i_stp.eefm_rot_damping_gain.length(stikp.size());
  i_stp.eefm_rot_compensation_limit.length(stikp.size());
+ i_stp.eefm_swing_rot_spring_gain.length(stikp.size());
  for (size_t j = 0; j < stikp.size(); j++) {
  i_stp.eefm_pos_damping_gain[j].length(3);
  i_stp.eefm_pos_time_const_support[j].length(3);
+ i_stp.eefm_swing_pos_spring_gain[j].length(3);
  i_stp.eefm_rot_damping_gain[j].length(3);
  i_stp.eefm_rot_time_const[j].length(3);
+ i_stp.eefm_swing_rot_spring_gain[j].length(3);
  for (size_t i = 0; i < 3; i++) {
  i_stp.eefm_pos_damping_gain[j][i] = stikp[j].eefm_pos_damping_gain(i);
  i_stp.eefm_pos_time_const_support[j][i] = stikp[j].eefm_pos_time_const_support(i);
+ i_stp.eefm_swing_pos_spring_gain[j][i] = stikp[j].eefm_swing_pos_spring_gain(i);
  i_stp.eefm_rot_damping_gain[j][i] = stikp[j].eefm_rot_damping_gain(i);
  i_stp.eefm_rot_time_const[j][i] = stikp[j].eefm_rot_time_const(i);
+ i_stp.eefm_swing_rot_spring_gain[j][i] = stikp[j].eefm_swing_rot_spring_gain(i);
  }
  i_stp.eefm_pos_compensation_limit[j] = stikp[j].eefm_pos_compensation_limit;
  i_stp.eefm_rot_compensation_limit[j] = stikp[j].eefm_rot_compensation_limit;
@@ -1655,16 +1663,20 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp)
  if ( i_stp.eefm_pos_damping_gain.length () == stikp.size() &&
  i_stp.eefm_pos_time_const_support.length () == stikp.size() &&
  i_stp.eefm_pos_compensation_limit.length () == stikp.size() &&
+ i_stp.eefm_swing_pos_spring_gain.length () == stikp.size() &&
  i_stp.eefm_rot_damping_gain.length () == stikp.size() &&
  i_stp.eefm_rot_time_const.length () == stikp.size() &&
- i_stp.eefm_rot_compensation_limit.length () == stikp.size() ) {
+ i_stp.eefm_rot_compensation_limit.length () == stikp.size() &&
+ i_stp.eefm_swing_rot_spring_gain.length () == stikp.size() ) {
  is_damping_parameter_ok = true;
  for (size_t j = 0; j < stikp.size(); j++) {
  for (size_t i = 0; i < 3; i++) {
  stikp[j].eefm_pos_damping_gain(i) = i_stp.eefm_pos_damping_gain[j][i];
  stikp[j].eefm_pos_time_const_support(i) = i_stp.eefm_pos_time_const_support[j][i];
+ stikp[j].eefm_swing_pos_spring_gain(i) = i_stp.eefm_swing_pos_spring_gain[j][i];
  stikp[j].eefm_rot_damping_gain(i) = i_stp.eefm_rot_damping_gain[j][i];
  stikp[j].eefm_rot_time_const(i) = i_stp.eefm_rot_time_const[j][i];
+ stikp[j].eefm_swing_rot_spring_gain(i) = i_stp.eefm_swing_rot_spring_gain[j][i];
  }
  stikp[j].eefm_pos_compensation_limit = i_stp.eefm_pos_compensation_limit[j];
  stikp[j].eefm_rot_compensation_limit = i_stp.eefm_rot_compensation_limit[j];
@@ -1772,6 +1784,9 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp)
  std::cerr << "[" << m_profile.instance_name << "] [" << stikp[j].ee_name << "] "
  << "eefm_pos_compensation_limit = " << stikp[j].eefm_pos_compensation_limit << "[m], " << " "
  << "eefm_rot_compensation_limit = " << stikp[j].eefm_rot_compensation_limit << "[rad]" << std::endl;
+ std::cerr << "[" << m_profile.instance_name << "] [" << stikp[j].ee_name << "] "
+ << "eefm_swing_pos_spring_gain = " << stikp[j].eefm_swing_pos_spring_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << ", "
+ << "eefm_swing_rot_spring_gain = " << stikp[j].eefm_swing_rot_spring_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << std::endl;
  }
  } else {
  std::cerr << "[" << m_profile.instance_name << "] eefm damping parameters cannot be set because of invalid param." << std::endl;

rtc/Stabilizer/Stabilizer.h

@@ -244,7 +244,7 @@ class Stabilizer
  hrp::Matrix33 localR; // Rotation of ee in end link frame (^{l}R_e = R_l^T R_e)
  // For eefm
  hrp::Vector3 d_foot_pos, d_foot_rpy, ee_d_foot_rpy;
- hrp::Vector3 eefm_pos_damping_gain, eefm_pos_time_const_support, eefm_rot_damping_gain, eefm_rot_time_const;
+ hrp::Vector3 eefm_pos_damping_gain, eefm_pos_time_const_support, eefm_rot_damping_gain, eefm_rot_time_const, eefm_swing_rot_spring_gain, eefm_swing_pos_spring_gain;
  double eefm_pos_compensation_limit, eefm_rot_compensation_limit;
  hrp::Vector3 ref_force, ref_moment;
  double swing_support_gain, support_time;