add a function to limit stride

idl/AutoBalancerService.idl

@@ -221,6 +221,10 @@ module OpenHRP
       long optional_go_pos_finalize_footstep_num;
       /// Offset for overwritable footstep index. Offset from current footstep index. Used in emergency_stop and velocity_mode.
       long overwritable_footstep_index_offset;
+      /// Stride limitation when overwriting footsteps (front, rear, outside, inside) [m]
+      DblArray4 overwritable_stride_limitation;
+      /// Use stride limitation or not
+      boolean use_stride_limitation;
     };
 
     /**

python/hrpsys_config.py

@@ -389,6 +389,7 @@ def connectComps(self):
             if self.es:
                 connectPorts(self.st.port("emergencySignal"), self.es.port("emergencySignal"))
             connectPorts(self.st.port("emergencySignal"), self.abc.port("emergencySignal"))
+            connectPorts(self.st.port("legMargin"), self.abc.port("legMargin"))
 
         # ref force moment connection
         for sen in self.getForceSensorNames():

rtc/AutoBalancer/AutoBalancer.cpp

@@ -62,6 +62,7 @@ AutoBalancer::AutoBalancer(RTC::Manager* manager)
       m_AutoBalancerServicePort("AutoBalancerService"),
       m_walkingStatesOut("walkingStates", m_walkingStates),
       m_sbpCogOffsetOut("sbpCogOffset", m_sbpCogOffset),
+      m_legMarginIn("legMargin", m_legMargin),
       // </rtc-template>
       gait_type(BIPED),
       move_base_gain(0.8),
@@ -90,6 +91,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     addInPort("zmpIn", m_zmpIn);
     addInPort("optionalData", m_optionalDataIn);
     addInPort("emergencySignal", m_emergencySignalIn);
+    addInPort("legMargin", m_legMarginIn);
 
     // Set OutPort buffer
     addOutPort("q", m_qOut);
@@ -442,6 +444,12 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
         //     gg->emergency_stop();
         // }
     }
+    if (m_legMarginIn.isNew()) {
+      m_legMarginIn.read();
+      for (size_t i = 0; i < 4; i++) {
+        gg->set_leg_margin(m_legMargin.data[i], i);
+      }
+    }
 
     Guard guard(m_mutex);
     hrp::Vector3 ref_basePos;
@@ -1412,6 +1420,8 @@ bool AutoBalancer::setGaitGeneratorParam(const OpenHRP::AutoBalancerService::Gai
   gg->set_zmp_weight_map(boost::assign::map_list_of<leg_type, double>(RLEG, i_param.zmp_weight_map[0])(LLEG, i_param.zmp_weight_map[1])(RARM, i_param.zmp_weight_map[2])(LARM, i_param.zmp_weight_map[3]));
   gg->set_optional_go_pos_finalize_footstep_num(i_param.optional_go_pos_finalize_footstep_num);
   gg->set_overwritable_footstep_index_offset(i_param.overwritable_footstep_index_offset);
+  gg->set_overwritable_stride_limitation(i_param.overwritable_stride_limitation);
+  gg->set_use_stride_limitation(i_param.use_stride_limitation);
 
   // print
   gg->print_param(std::string(m_profile.instance_name));
@@ -1481,6 +1491,10 @@ bool AutoBalancer::getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGener
   i_param.zmp_weight_map[3] = tmp_zmp_weight_map[LARM];
   i_param.optional_go_pos_finalize_footstep_num = gg->get_optional_go_pos_finalize_footstep_num();
   i_param.overwritable_footstep_index_offset = gg->get_overwritable_footstep_index_offset();
+  for (size_t i=0; i<4; i++) {
+    i_param.overwritable_stride_limitation[i] = gg->get_overwritable_stride_limitation(i);
+  }
+  i_param.use_stride_limitation = gg->get_use_stride_limitation();
   return true;
 };
 

rtc/AutoBalancer/AutoBalancer.h

@@ -133,6 +133,8 @@ class AutoBalancer
   std::vector<InPort<TimedDoubleSeq> *> m_ref_forceIn;
   TimedLong m_emergencySignal;
   InPort<TimedLong> m_emergencySignalIn;
+  TimedDoubleSeq m_legMargin;
+  InPort<TimedDoubleSeq> m_legMarginIn;
   // for debug
   TimedPoint3D m_cog;
 

rtc/AutoBalancer/GaitGenerator.cpp

@@ -578,6 +578,22 @@ namespace rats
         overwrite_footstep_nodes_list.clear();
       }
     }
+    // limit stride
+    if (use_stride_limitation && lcg.get_footstep_index() > 0 && lcg.get_footstep_index() < footstep_nodes_list.size()-overwritable_footstep_index_offset-2 &&
+        (overwritable_footstep_index_offset == 0 || lcg.get_lcg_count() == get_overwrite_check_timing())) {
+      if (lcg.get_footstep_index() == footstep_nodes_list.size()-overwritable_footstep_index_offset-3) {
+        hrp::Vector3 orig_footstep_pos = footstep_nodes_list[get_overwritable_index()].front().worldcoords.pos;
+        limit_stride(footstep_nodes_list[get_overwritable_index()].front(), footstep_nodes_list[get_overwritable_index()-1].front());
+        for (size_t i = get_overwritable_index() + 1; i < footstep_nodes_list.size(); i++) {
+          footstep_nodes_list[i].front().worldcoords.pos -= orig_footstep_pos - footstep_nodes_list[get_overwritable_index()].front().worldcoords.pos;
+        }
+      } else {
+        limit_stride(footstep_nodes_list[get_overwritable_index()].front(), footstep_nodes_list[get_overwritable_index()-1].front());
+      }
+      overwrite_footstep_nodes_list.insert(overwrite_footstep_nodes_list.end(), footstep_nodes_list.begin()+get_overwritable_index(), footstep_nodes_list.end());
+      overwrite_refzmp_queue(overwrite_footstep_nodes_list);
+      overwrite_footstep_nodes_list.clear();
+    }
 
     if ( !solved ) {
       hrp::Vector3 rzmp;
@@ -614,6 +630,37 @@ namespace rats
     return solved;
   };
 
+  void gait_generator::limit_stride (step_node& cur_fs, const step_node& prev_fs)
+  {
+    leg_type cur_leg = cur_fs.l_r;
+    // prev_fs frame
+    cur_fs.worldcoords.pos = prev_fs.worldcoords.rot.transpose() * (cur_fs.worldcoords.pos - prev_fs.worldcoords.pos);
+    double stride_r = std::pow(cur_fs.worldcoords.pos(0), 2.0) + std::pow(cur_fs.worldcoords.pos(1) + (cur_leg == LLEG ? -1 : 1) * overwritable_stride_limitation[3], 2.0);
+    // front, rear, outside limitation
+    double stride_r_limit = std::pow(std::max(overwritable_stride_limitation[cur_fs.worldcoords.pos(0) >= 0 ? 0 : 1], overwritable_stride_limitation[2] - overwritable_stride_limitation[3]), 2.0);
+    if (stride_r > stride_r_limit) {
+        cur_fs.worldcoords.pos(0) *= sqrt(stride_r_limit / stride_r);
+        cur_fs.worldcoords.pos(1) = (cur_leg == LLEG ? 1 : -1) * overwritable_stride_limitation[3] + sqrt(stride_r_limit / stride_r) * (cur_fs.worldcoords.pos(1) + (cur_leg == LLEG ? -1 : 1) * overwritable_stride_limitation[3]);
+    }
+    if (cur_fs.worldcoords.pos(0) > overwritable_stride_limitation[0]) cur_fs.worldcoords.pos(0) = overwritable_stride_limitation[0];
+    if (cur_fs.worldcoords.pos(0) < -1 * overwritable_stride_limitation[0]) cur_fs.worldcoords.pos(0) = -1 * overwritable_stride_limitation[1];
+    if ((cur_leg == LLEG ? 1 : -1) * cur_fs.worldcoords.pos(1) > overwritable_stride_limitation[2]) cur_fs.worldcoords.pos(1) = (cur_leg == LLEG ? 1 : -1) * overwritable_stride_limitation[2];
+    // inside limitation
+    std::vector<double> cur_leg_vertices_y;
+    cur_leg_vertices_y.reserve(4);
+    cur_leg_vertices_y.push_back((cur_fs.worldcoords.pos + prev_fs.worldcoords.rot.transpose() * cur_fs.worldcoords.rot * hrp::Vector3(leg_margin[0], (cur_leg == LLEG ? 1 : -1) * leg_margin[2], 0.0))(1));
+    cur_leg_vertices_y.push_back((cur_fs.worldcoords.pos + prev_fs.worldcoords.rot.transpose() * cur_fs.worldcoords.rot * hrp::Vector3(leg_margin[0], (cur_leg == LLEG ? -1 : 1) * leg_margin[3], 0.0))(1));
+    cur_leg_vertices_y.push_back((cur_fs.worldcoords.pos + prev_fs.worldcoords.rot.transpose() * cur_fs.worldcoords.rot * hrp::Vector3(-1 * leg_margin[1], (cur_leg == LLEG ? 1 : -1) * leg_margin[2], 0.0))(1));
+    cur_leg_vertices_y.push_back((cur_fs.worldcoords.pos + prev_fs.worldcoords.rot.transpose() * cur_fs.worldcoords.rot * hrp::Vector3(-1 * leg_margin[1], (cur_leg == LLEG ? -1 : 1) * leg_margin[3], 0.0))(1));
+    if (cur_leg == LLEG) {
+      if (*std::min_element(cur_leg_vertices_y.begin(), cur_leg_vertices_y.end()) < overwritable_stride_limitation[3]) cur_fs.worldcoords.pos(1) += overwritable_stride_limitation[3] - *std::min_element(cur_leg_vertices_y.begin(), cur_leg_vertices_y.end());
+    } else {
+      if (*std::max_element(cur_leg_vertices_y.begin(), cur_leg_vertices_y.end()) > -1 * overwritable_stride_limitation[3]) cur_fs.worldcoords.pos(1) += -1 * overwritable_stride_limitation[3] - *std::max_element(cur_leg_vertices_y.begin(), cur_leg_vertices_y.end());
+    }
+    // world frame
+    cur_fs.worldcoords.pos = prev_fs.worldcoords.pos + prev_fs.worldcoords.rot * cur_fs.worldcoords.pos;
+  };
+
   /* 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

@@ -880,6 +880,8 @@ namespace rats
     std::map<leg_type, std::string> leg_type_map;
     coordinates initial_foot_mid_coords;
     bool solved;
+    double leg_margin[4], overwritable_stride_limitation[4];
+    bool use_stride_limitation;
 
     /* preview controller parameters */
     //preview_dynamics_filter<preview_control>* preview_controller_ptr;
@@ -926,11 +928,13 @@ namespace rats
         dt(_dt), 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_inside_step_limitation(true), use_stride_limitation(false),
         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;
     };
     ~gait_generator () {
       if ( preview_controller_ptr != NULL ) {
@@ -943,6 +947,7 @@ namespace rats
                                     const std::vector<step_node>& initial_swing_leg_dst_steps,
                                     const double delay = 1.6);
     bool proc_one_tick ();
+    void limit_stride (step_node& cur_fs, const step_node& prev_fs);
     void append_footstep_nodes (const std::vector<std::string>& _legs, const std::vector<coordinates>& _fss)
     {
         std::vector<step_node> tmp_sns;
@@ -1072,6 +1077,15 @@ namespace rats
         append_finalize_footstep(overwrite_footstep_nodes_list);
         print_footstep_nodes_list(overwrite_footstep_nodes_list);
     };
+    void set_leg_margin (const double _leg_margin, const size_t idx) {
+        leg_margin[idx] = _leg_margin;
+    };
+    void set_overwritable_stride_limitation (const double _overwritable_stride_limitation[4]) {
+      for (size_t i = 0; i < 4; i++) {
+        overwritable_stride_limitation[i] = _overwritable_stride_limitation[i];
+      }
+    };
+    void set_use_stride_limitation (const bool _use_stride_limitation) { use_stride_limitation = _use_stride_limitation; };
     /* 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
     {
@@ -1223,6 +1237,8 @@ namespace rats
     size_t get_optional_go_pos_finalize_footstep_num () const { return optional_go_pos_finalize_footstep_num; };
     bool is_finalizing (const double tm) const { return ((preview_controller_ptr->get_delay()*2 - default_step_time/dt)-finalize_count) <= (tm/dt)-1; };
     size_t get_overwrite_check_timing () const { return static_cast<size_t>(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * 0.5) - 1;}; // Almost middle of step time
+    double get_overwritable_stride_limitation (const size_t idx) const { return overwritable_stride_limitation[idx]; };
+    bool get_use_stride_limitation () const { return use_stride_limitation; };
     void print_param (const std::string& print_str = "") const
     {
         double stride_fwd_x, stride_y, stride_th, stride_bwd_x;

rtc/Stabilizer/Stabilizer.cpp

@@ -62,6 +62,7 @@ Stabilizer::Stabilizer(RTC::Manager* manager)
     m_actContactStatesOut("actContactStates", m_actContactStates),
     m_COPInfoOut("COPInfo", m_COPInfo),
     m_emergencySignalOut("emergencySignal", m_emergencySignal),
+    m_legMarginOut("legMargin", m_legMargin),
     // for debug output
     m_originRefZmpOut("originRefZmp", m_originRefZmp),
     m_originRefCogOut("originRefCog", m_originRefCog),
@@ -123,6 +124,7 @@ RTC::ReturnCode_t Stabilizer::onInitialize()
   addOutPort("actContactStates", m_actContactStatesOut);
   addOutPort("COPInfo", m_COPInfoOut);
   addOutPort("emergencySignal", m_emergencySignalOut);
+  addOutPort("legMargin", m_legMarginOut);
   // for debug output
   addOutPort("originRefZmp", m_originRefZmpOut);
   addOutPort("originRefCog", m_originRefCogOut);
@@ -389,6 +391,7 @@ RTC::ReturnCode_t Stabilizer::onInitialize()
   total_mass = m_robot->totalMass();
   ref_zmp_aux = hrp::Vector3::Zero();
   m_actContactStates.data.length(m_contactStates.data.length());
+  m_legMargin.data.length(4);
   for (size_t i = 0; i < m_contactStates.data.length(); i++) {
     contact_states.push_back(true);
     prev_contact_states.push_back(true);
@@ -597,6 +600,12 @@ RTC::ReturnCode_t Stabilizer::onExecute(RTC::UniqueId ec_id)
       m_actContactStatesOut.write();
       m_COPInfo.tm = m_qRef.tm;
       m_COPInfoOut.write();
+      m_legMargin.data[0] = szd->get_leg_front_margin();
+      m_legMargin.data[1] = szd->get_leg_rear_margin();
+      m_legMargin.data[2] = szd->get_leg_outside_margin();
+      m_legMargin.data[3] = szd->get_leg_inside_margin();
+      m_legMargin.tm = m_qRef.tm;
+      m_legMarginOut.write();
       //m_tauOut.write();
       // for debug output
       m_originRefZmp.data.x = ref_zmp(0); m_originRefZmp.data.y = ref_zmp(1); m_originRefZmp.data.z = ref_zmp(2);

rtc/Stabilizer/Stabilizer.h

@@ -163,6 +163,7 @@ class Stabilizer
   RTC::TimedDoubleSeq m_qRefSeq;
   RTC::TimedBoolean m_walkingStates;
   RTC::TimedPoint3D m_sbpCogOffset;
+  RTC::TimedDoubleSeq m_legMargin;
   // for debug ouput
   RTC::TimedPoint3D m_originRefZmp, m_originRefCog, m_originRefCogVel, m_originNewZmp;
   RTC::TimedPoint3D m_originActZmp, m_originActCog, m_originActCogVel;
@@ -205,6 +206,7 @@ class Stabilizer
   RTC::OutPort<RTC::TimedBooleanSeq> m_actContactStatesOut;
   RTC::OutPort<RTC::TimedDoubleSeq> m_COPInfoOut;
   RTC::OutPort<RTC::TimedLong> m_emergencySignalOut;
+  RTC::OutPort<RTC::TimedDoubleSeq> m_legMarginOut;
   // for debug output
   RTC::OutPort<RTC::TimedPoint3D> m_originRefZmpOut, m_originRefCogOut, m_originRefCogVelOut, m_originNewZmpOut;
   RTC::OutPort<RTC::TimedPoint3D> m_originActZmpOut, m_originActCogOut, m_originActCogVelOut;