interpolate root height to avoid limb stretching

idl/AutoBalancerService.idl

@@ -301,6 +301,8 @@ module OpenHRP
       GaitType default_gait_type;
       /// Sequence for all end-effectors' ik limb parameters
       sequence<IKLimbParameters> ik_limb_parameters;
+      /// Sequence of limb length margin from max limb length [m]
+      sequence<double> limb_length_margin;
     };
 
     /**

python/hrpsys_config.py

@@ -393,6 +393,7 @@ def connectComps(self):
             connectPorts(self.abc.port("walkingStates"), self.st.port("walkingStates"))
             connectPorts(self.abc.port("sbpCogOffset"), self.st.port("sbpCogOffset"))
             connectPorts(self.abc.port("toeheelRatio"), self.st.port("toeheelRatio"))
+            connectPorts(self.abc.port("interpolatedRootHeight"), self.st.port("interpolatedRootHeight"))
             if self.es:
                 connectPorts(self.st.port("emergencySignal"), self.es.port("emergencySignal"))
             connectPorts(self.st.port("emergencySignal"), self.abc.port("emergencySignal"))

rtc/AutoBalancer/AutoBalancer.cpp

@@ -62,6 +62,7 @@ AutoBalancer::AutoBalancer(RTC::Manager* manager)
       m_walkingStatesOut("walkingStates", m_walkingStates),
       m_sbpCogOffsetOut("sbpCogOffset", m_sbpCogOffset),
       m_cogOut("cogOut", m_cog),
+      m_interpolatedRootHeightOut("interpolatedRootHeight", m_interpolatedRootHeight),
       m_AutoBalancerServicePort("AutoBalancerService"),
       // </rtc-template>
       gait_type(BIPED),
@@ -106,6 +107,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     addOutPort("cogOut", m_cogOut);
     addOutPort("walkingStates", m_walkingStatesOut);
     addOutPort("sbpCogOffset", m_sbpCogOffsetOut);
+    addOutPort("interpolatedRootHeight", m_interpolatedRootHeightOut);
 
     // Set service provider to Ports
     m_AutoBalancerServicePort.registerProvider("service0", "AutoBalancerService", m_service0);
@@ -168,6 +170,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
         coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str());
         coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
         ABCIKparam tp;
+        hrp::Link* root = m_robot->link(ee_target);
         for (size_t j = 0; j < 3; j++) {
           coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
         }
@@ -192,6 +195,13 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
         tp.avoid_gain = 0.001;
         tp.reference_gain = 0.01;
         tp.pos_ik_error_count = tp.rot_ik_error_count = 0;
+        tp.max_limb_length = 0.0;
+        while (!root->isRoot()) {
+          tp.max_limb_length += root->b.norm();
+          tp.parent_name = root->name;
+          root = root->parent;
+        }
+        tp.limb_length_margin = 0.13;
         ikp.insert(std::pair<std::string, ABCIKparam>(ee_name , tp));
         ikp[ee_name].target_link = m_robot->link(ee_target);
         ee_vec.push_back(ee_name);
@@ -252,6 +262,8 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     leg_names_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
     leg_names_interpolator->setName(std::string(m_profile.instance_name)+" leg_names_interpolator");
     leg_names_interpolator_ratio = 1.0;
+    limb_stretch_avoidance_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
+    limb_stretch_avoidance_interpolator->setName(std::string(m_profile.instance_name)+" limb_stretch_avoidance_interpolator");
 
     // setting stride limitations from conf file
     double stride_fwd_x_limit = 0.15;
@@ -349,6 +361,8 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     rot_ik_thre = (1e-2)*M_PI/180.0; // [rad]
     ik_error_debug_print_freq = static_cast<int>(0.2/m_dt); // once per 0.2 [s]
 
+    prev_d_root_z_pos = 0.0;
+
     hrp::Sensor* sen = m_robot->sensor<hrp::RateGyroSensor>("gyrometer");
     if (sen == NULL) {
         std::cerr << "[" << m_profile.instance_name << "] WARNING! This robot model has no GyroSensor named 'gyrometer'! " << std::endl;
@@ -364,6 +378,7 @@ RTC::ReturnCode_t AutoBalancer::onFinalize()
   delete transition_interpolator;
   delete adjust_footstep_interpolator;
   delete leg_names_interpolator;
+  delete limb_stretch_avoidance_interpolator;
   return RTC::RTC_OK;
 }
 
@@ -498,6 +513,7 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
         std::cerr << "[" << m_profile.instance_name << "] Finished cleanup" << std::endl;
         control_mode = MODE_IDLE;
       }
+      interpolateLimbStretch();
     }
     if ( m_qRef.data.length() != 0 ) { // initialized
       if (is_legged_robot) {
@@ -581,6 +597,8 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
     m_walkingStates.data = gg_is_walking;
     m_walkingStates.tm = m_qRef.tm;
     m_walkingStatesOut.write();
+    m_interpolatedRootHeight.tm = m_qRef.tm;
+    m_interpolatedRootHeightOut.write();
 
     for (unsigned int i=0; i<m_ref_forceOut.size(); i++){
         m_force[i].tm = m_qRef.tm;
@@ -956,6 +974,30 @@ void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matri
   m_robot->calcForwardKinematics();
 }
 
+void AutoBalancer::interpolateLimbStretch ()
+{
+  std::vector<hrp::Vector3> future_d_ee_pos = gg->get_future_d_ee_pos();
+  double tmp_d_root_z_pos = 0.0, tmp_time = gg->get_limb_stretch_remain_time() + m_dt;
+  static double prev_v;
+  for (size_t i = 0; i < leg_names.size(); i++) {
+    // Check whether inside limb length limitation
+    hrp::Link* parent_link = m_robot->link(ikp[leg_names[i]].parent_name);
+    hrp::Vector3 future_targetp = ikp[leg_names[i]].target_p0 + future_d_ee_pos[i] - parent_link->p; // position from parent to target link in future (world frame)
+    double limb_length_limitation = ikp[leg_names[i]].max_limb_length - ikp[leg_names[i]].limb_length_margin;
+    double tmp = limb_length_limitation * limb_length_limitation - future_targetp(0) * future_targetp(0) - future_targetp(1) * future_targetp(1);
+    if (future_targetp.norm() > limb_length_limitation && tmp >= 0) {
+      tmp_d_root_z_pos = std::min(tmp_d_root_z_pos, future_targetp(2) + std::sqrt(tmp));
+    }
+  }
+  gg->clear_future_d_ee_pos();
+  limb_stretch_avoidance_interpolator->set(&prev_d_root_z_pos, &prev_v);
+  limb_stretch_avoidance_interpolator->setGoal(&tmp_d_root_z_pos, tmp_time, true);
+  for (size_t i = 0; i < 2; i++) {
+    limb_stretch_avoidance_interpolator->get(&d_root_z_pos, &prev_v, true);
+  }
+  m_interpolatedRootHeight.data = prev_d_root_z_pos = std::min(0.0, d_root_z_pos);
+}
+
 bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param, const std::string& limb_name)
 {
   param.manip->calcInverseKinematics2Loop(param.target_p0, param.target_r0, 1.0, param.avoid_gain, param.reference_gain, &qrefv, transition_interpolator_ratio * leg_names_interpolator_ratio);
@@ -1646,6 +1688,9 @@ bool AutoBalancer::setAutoBalancerParam(const OpenHRP::AutoBalancerService::Auto
   } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::GALLOP) {
       gait_type = GALLOP;
   }
+  for (size_t i = 0; i < ikp.size(); i++) {
+    ikp[ee_vec[i]].limb_length_margin = i_param.limb_length_margin[i];
+  }
   for (std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++) {
       std::cerr << "[" << m_profile.instance_name << "] End Effector [" << it->first << "]" << std::endl;
       std::cerr << "[" << m_profile.instance_name << "]   localpos = " << it->second.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
@@ -1749,11 +1794,13 @@ bool AutoBalancer::getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalanc
 {
   i_param.move_base_gain = move_base_gain;
   i_param.default_zmp_offsets.length(ikp.size());
+  i_param.limb_length_margin.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);
       }
+      i_param.limb_length_margin[i] = ikp[ee_vec[i]].limb_length_margin;
   }
   switch(control_mode) {
   case MODE_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_IDLE; break;

rtc/AutoBalancer/AutoBalancer.h

@@ -166,6 +166,8 @@ class AutoBalancer
   std::vector<OutPort<TimedDoubleSeq> *> m_ref_forceOut;
   std::vector<TimedPoint3D> m_limbCOPOffset;
   std::vector<OutPort<TimedPoint3D> *> m_limbCOPOffsetOut;
+  TimedDouble m_interpolatedRootHeight;
+  OutPort<TimedDouble> m_interpolatedRootHeightOut;
   // for debug
   OutPort<TimedPoint3D> m_cogOut;
 
@@ -192,10 +194,11 @@ class AutoBalancer
   struct ABCIKparam {
     hrp::Vector3 target_p0, localPos, adjust_interpolation_target_p0, adjust_interpolation_org_p0;
     hrp::Matrix33 target_r0, localR, adjust_interpolation_target_r0, adjust_interpolation_org_r0;
+    std::string parent_name; // Name of parent ling in the limb
     rats::coordinates target_end_coords;
     hrp::Link* target_link;
     hrp::JointPathExPtr manip;
-    double avoid_gain, reference_gain;
+    double avoid_gain, reference_gain, max_limb_length, limb_length_margin;
     size_t pos_ik_error_count, rot_ik_error_count;
     bool is_active, has_toe_joint;
   };
@@ -208,6 +211,7 @@ class AutoBalancer
   void waitABCTransition();
   hrp::Matrix33 OrientRotationMatrix (const hrp::Matrix33& rot, const hrp::Vector3& axis1, const hrp::Vector3& axis2);
   void fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matrix33& fix_rot);
+  void interpolateLimbStretch ();
   void startWalking ();
   void stopWalking ();
   void copyRatscoords2Footstep(OpenHRP::AutoBalancerService::Footstep& out_fs, const rats::coordinates& in_fs);
@@ -241,12 +245,14 @@ class AutoBalancer
   double m_dt, move_base_gain;
   hrp::BodyPtr m_robot;
   coil::Mutex m_mutex;
+  double d_root_z_pos, prev_d_root_z_pos;
 
   double transition_interpolator_ratio, transition_time, zmp_transition_time, adjust_footstep_transition_time, leg_names_interpolator_ratio;
   interpolator *zmp_offset_interpolator;
   interpolator *transition_interpolator;
   interpolator *adjust_footstep_interpolator;
   interpolator *leg_names_interpolator;
+  interpolator *limb_stretch_avoidance_interpolator;
   hrp::Vector3 input_zmp, input_basePos;
   hrp::Matrix33 input_baseRot;
 

rtc/AutoBalancer/GaitGenerator.cpp

@@ -638,6 +638,8 @@ namespace rats
     //   std::cerr << ")" << std::endl;
     // }
 
+    calc_future_cog();
+
     /* update swing_leg_coords, support_leg_coords */
     if ( solved ) {
       lcg.update_leg_steps(footstep_nodes_list, default_double_support_ratio_swing_before, default_double_support_ratio_swing_after, thtc);
@@ -679,6 +681,48 @@ namespace rats
     cur_fs.worldcoords.pos = prev_fs.worldcoords.pos + prev_fs.worldcoords.rot * cur_fs.worldcoords.pos;
   };
 
+  /* calculate future cog when touching down and taking off
+   * and difference between current and future end-effector position
+   * based on LIPM daynamics
+   */
+  void gait_generator::calc_future_cog ()
+  {
+    double omega = std::sqrt(gravitational_acceleration / cog(2) - refzmp(2));
+    std::vector<step_node> swg_leg_stps = lcg.get_swing_leg_steps();
+    if (lcg.get_lcg_count() < static_cast<size_t>(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * (1.0 - default_double_support_ratio_before)) - 1
+        && lcg.get_lcg_count() >= static_cast<size_t>(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * default_double_support_ratio_after) - 1) { // single support phase
+      limb_stretch_remain_time = static_cast<double>(lcg.get_lcg_count() + 1) * dt - footstep_nodes_list[lcg.get_footstep_index()][0].step_time * default_double_support_ratio_after;
+      hrp::Vector3 future_cog = (cog - refzmp) * std::cosh(omega * limb_stretch_remain_time) + (cog - prev_cog) / dt / omega * std::sinh(omega * limb_stretch_remain_time) + refzmp;
+      future_cog(2) = cog(2);
+      for (size_t i = 0; i < all_limbs.size(); i++) {
+        bool is_swing = false;
+        for (size_t j = 0; j < swg_leg_stps.size(); j++) {
+          if (all_limbs[i] ==  leg_type_map[swg_leg_stps[j].l_r]) {
+            future_d_ee_pos[i] = (cog - swg_leg_stps[j].worldcoords.pos) - (future_cog - footstep_nodes_list[lcg.get_footstep_index()][j].worldcoords.pos);
+            is_swing = true;
+          }
+        }
+        if (!is_swing) future_d_ee_pos[i] = cog - future_cog;
+      }
+    } else if (lcg.get_lcg_count() >= static_cast<size_t>(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * (1.0 - default_double_support_ratio_before)) - 1) { // double support (before) phase
+      limb_stretch_remain_time = static_cast<double>(lcg.get_lcg_count() + 1) * dt - footstep_nodes_list[lcg.get_footstep_index()][0].step_time * (1.0 - default_double_support_ratio_before);
+      hrp::Vector3 future_cog = (cog - refzmp) * std::cosh(omega * limb_stretch_remain_time) + ((cog - prev_cog)/dt - (refzmp - prev_refzmp)/dt) / omega * std::sinh(omega * limb_stretch_remain_time) + refzmp + (refzmp - prev_refzmp)/dt * limb_stretch_remain_time;
+      future_cog(2) = cog(2);
+      for (size_t i = 0; i < all_limbs.size(); i++) {
+        future_d_ee_pos[i] = cog - future_cog;
+      }
+    } else {  // double support (after) phase
+      limb_stretch_remain_time = static_cast<double>(lcg.get_lcg_count() + 1) * dt + footstep_nodes_list[lcg.get_footstep_index() + (get_footstep_index()<footstep_nodes_list.size()-1?1:0)][0].step_time * default_double_support_ratio_before;
+      hrp::Vector3 future_cog = (cog - refzmp) * std::cosh(omega * limb_stretch_remain_time) + ((cog - prev_cog)/dt - (refzmp - prev_refzmp)/dt) / omega * std::sinh(omega * limb_stretch_remain_time) + refzmp + (refzmp - prev_refzmp)/dt * limb_stretch_remain_time;
+      future_cog(2) = cog(2);
+      for (size_t i = 0; i < all_limbs.size(); i++) {
+        future_d_ee_pos[i] = cog - future_cog;
+      }
+    }
+    prev_cog = cog;
+    prev_refzmp = refzmp;
+  };
+
   /* generate vector of step_node from :go-pos params
    *  x, y and theta are simply divided by using stride params
    *  unit system -> x [mm], y [mm], theta [deg]

rtc/AutoBalancer/GaitGenerator.h

@@ -1029,7 +1029,7 @@ namespace rats
     footstep_parameter footstep_param;
     velocity_mode_parameter vel_param, offset_vel_param;
     toe_heel_type_checker thtc;
-    hrp::Vector3 cog, refzmp, prev_que_rzmp; /* cog by calculating proc_one_tick */
+    hrp::Vector3 cog, prev_cog, refzmp, prev_refzmp, prev_que_rzmp; /* cog by calculating proc_one_tick */
     std::vector<hrp::Vector3> swing_foot_zmp_offsets, prev_que_sfzos;
     double dt; /* control loop [s] */
     std::vector<std::string> all_limbs;
@@ -1054,6 +1054,8 @@ namespace rats
     double leg_margin[4], overwritable_stride_limitation[4];
     bool use_stride_limitation;
     stride_limitation_type default_stride_limitation_type;
+    double limb_stretch_remain_time;
+    std::vector<hrp::Vector3> future_d_ee_pos;
 
     /* preview controller parameters */
     //preview_dynamics_filter<preview_control>* preview_controller_ptr;
@@ -1096,17 +1098,18 @@ namespace rats
                     const double _stride_fwd_x, const double _stride_y, const double _stride_theta, const double _stride_bwd_x)
         : footstep_nodes_list(), overwrite_footstep_nodes_list(), rg(_dt), lcg(_dt),
         footstep_param(_leg_pos, _stride_fwd_x, _stride_y, _stride_theta, _stride_bwd_x),
-        vel_param(), offset_vel_param(), thtc(), cog(hrp::Vector3::Zero()), refzmp(hrp::Vector3::Zero()), prev_que_rzmp(hrp::Vector3::Zero()),
+        vel_param(), offset_vel_param(), thtc(), cog(hrp::Vector3::Zero()), prev_cog(hrp::Vector3::Zero()), refzmp(hrp::Vector3::Zero()), prev_refzmp(hrp::Vector3::Zero()), prev_que_rzmp(hrp::Vector3::Zero()),
         dt(_dt), all_limbs(_all_limbs), default_step_time(1.0), default_double_support_ratio_before(0.1), default_double_support_ratio_after(0.1), default_double_support_static_ratio_before(0.0), default_double_support_static_ratio_after(0.0), default_double_support_ratio_swing_before(0.1), default_double_support_ratio_swing_after(0.1), gravitational_acceleration(DEFAULT_GRAVITATIONAL_ACCELERATION),
         finalize_count(0), optional_go_pos_finalize_footstep_num(0), overwrite_footstep_index(0), overwritable_footstep_index_offset(1),
         velocity_mode_flg(VEL_IDLING), emergency_flg(IDLING),
-        use_inside_step_limitation(true), use_stride_limitation(false), default_stride_limitation_type(SQUARE),
+        use_inside_step_limitation(true), use_stride_limitation(false), default_stride_limitation_type(SQUARE), limb_stretch_remain_time(0.0),
         preview_controller_ptr(NULL) {
         swing_foot_zmp_offsets = boost::assign::list_of<hrp::Vector3>(hrp::Vector3::Zero());
         prev_que_sfzos = boost::assign::list_of<hrp::Vector3>(hrp::Vector3::Zero());
         leg_type_map = boost::assign::map_list_of<leg_type, std::string>(RLEG, "rleg")(LLEG, "lleg")(RARM, "rarm")(LARM, "larm");
         for (size_t i = 0; i < 4; i++) leg_margin[i] = 0.1;
         for (size_t i = 0; i < 4; i++) overwritable_stride_limitation[i] = 0.2;
+        future_d_ee_pos.resize(_all_limbs.size(), hrp::Vector3::Zero());
     };
     ~gait_generator () {
       if ( preview_controller_ptr != NULL ) {
@@ -1120,6 +1123,7 @@ namespace rats
                                     const double delay = 1.6);
     bool proc_one_tick ();
     void limit_stride (step_node& cur_fs, const step_node& prev_fs) const;
+    void calc_future_cog ();
     void append_footstep_nodes (const std::vector<std::string>& _legs, const std::vector<coordinates>& _fss)
     {
         std::vector<step_node> tmp_sns;
@@ -1270,6 +1274,7 @@ namespace rats
     void set_stride_limitation_type (const stride_limitation_type _tmp) { default_stride_limitation_type = _tmp; };
     void set_toe_check_thre (const double _a) { thtc.set_toe_check_thre(_a); };
     void set_heel_check_thre (const double _a) { thtc.set_heel_check_thre(_a); };
+    void clear_future_d_ee_pos () { future_d_ee_pos.assign(all_limbs.size(), hrp::Vector3::Zero()); };
     /* Get overwritable footstep index. For example, if overwritable_footstep_index_offset = 1, overwrite next footstep. If overwritable_footstep_index_offset = 0, overwrite current swinging footstep. */
     size_t get_overwritable_index () const
     {
@@ -1430,6 +1435,8 @@ namespace rats
     stride_limitation_type get_stride_limitation_type () const { return default_stride_limitation_type; };
     double get_toe_check_thre () const { return thtc.get_toe_check_thre(); };
     double get_heel_check_thre () const { return thtc.get_heel_check_thre(); };
+    std::vector<hrp::Vector3> get_future_d_ee_pos () const { return future_d_ee_pos; };
+    double get_limb_stretch_remain_time () const { return limb_stretch_remain_time; };
     void print_param (const std::string& print_str = "") const
     {
         double stride_fwd_x, stride_y, stride_th, stride_bwd_x;