Merge pull request #553 from snozawa/update_abc_cog_foot

Add toe-off and heel-contact

idl/AutoBalancerService.idl

@@ -115,6 +115,16 @@ module OpenHRP
       DblArray3 stair_trajectory_way_point_offset;
       /// Gravitational acceleration [m/s^2]
       double gravitational_acceleration;
+      /// Toe position offset [mm] in end-effector frame x axis
+      double toe_pos_offset_x;
+      /// Heel position offset [mm] in end-effector frame x axis
+      double heel_pos_offset_x;
+      /// Maximum toe angle [deg] for toe-off motion
+      double toe_angle;
+      /// Maximum heel angle [deg] for heel-contact motion
+      double heel_angle;
+      /// Sequence of phase ratio of toe-off and heel-contact. Sum of toe_heel_phase_ratio should be 1.0.
+      sequence<double> toe_heel_phase_ratio;
     };
 
     /**

rtc/AutoBalancer/AutoBalancer.cpp

@@ -992,6 +992,16 @@ bool AutoBalancer::setGaitGeneratorParam(const OpenHRP::AutoBalancerService::Gai
   gg->set_swing_trajectory_delay_time_offset(i_param.swing_trajectory_delay_time_offset);
   gg->set_stair_trajectory_way_point_offset(hrp::Vector3(i_param.stair_trajectory_way_point_offset[0], i_param.stair_trajectory_way_point_offset[1], i_param.stair_trajectory_way_point_offset[2]));
   gg->set_gravitational_acceleration(i_param.gravitational_acceleration);
+  gg->set_toe_angle(i_param.toe_angle);
+  gg->set_heel_angle(i_param.heel_angle);
+  gg->set_toe_pos_offset_x(i_param.toe_pos_offset_x);
+  gg->set_heel_pos_offset_x(i_param.heel_pos_offset_x);
+  if (i_param.toe_heel_phase_ratio.length() == gg->get_NUM_TH_PHASES()) {
+      double ratio[gg->get_NUM_TH_PHASES()];
+      for (int i = 0; i < gg->get_NUM_TH_PHASES(); i++) ratio[i] = i_param.toe_heel_phase_ratio[i];
+      gg->set_toe_heel_phase_ratio(ratio);
+  }
+
   // print
   double stride_fwd_x, stride_y, stride_th, stride_bwd_x;
   gg->get_stride_parameters(stride_fwd_x, stride_y, stride_th, stride_bwd_x);
@@ -1015,6 +1025,19 @@ bool AutoBalancer::setGaitGeneratorParam(const OpenHRP::AutoBalancerService::Gai
   tmpv = gg->get_stair_trajectory_way_point_offset();
   std::cerr << "[" << m_profile.instance_name << "]   stair_trajectory_way_point_offset = " << tmpv.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
   std::cerr << "[" << m_profile.instance_name << "]   gravitational_acceleration = " << gg->get_gravitational_acceleration() << "[m/s^2]" << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   toe_pos_offset_x = " << gg->get_toe_pos_offset_x() << "[mm]" << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   heel_pos_offset_x = " << gg->get_heel_pos_offset_x() << "[mm]" << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   toe_angle = " << gg->get_toe_angle() << "[deg]" << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   heel_angle = " << gg->get_heel_angle() << "[deg]" << std::endl;
+  if (i_param.toe_heel_phase_ratio.length() == gg->get_NUM_TH_PHASES()) {
+      double ratio[gg->get_NUM_TH_PHASES()];
+      gg->get_toe_heel_phase_ratio(ratio);
+      std::cerr << "[" << m_profile.instance_name << "]   toe_heel_phase_ratio = [";
+      for (int i = 0; i < gg->get_NUM_TH_PHASES(); i++) std::cerr << ratio[i] << " ";
+      std::cerr << "]" << std::endl;
+  } else {
+      std::cerr << "[" << m_profile.instance_name << "]   toe_heel_phase_ratio is not set. Required length = " << gg->get_NUM_TH_PHASES() << " != input length " << i_param.toe_heel_phase_ratio.length() << std::endl;
+  }
   return true;
 };
 
@@ -1037,6 +1060,13 @@ bool AutoBalancer::getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGener
   for (size_t i = 0; i < 3; i++) i_param.stair_trajectory_way_point_offset[i] = tmpv(i);
   i_param.swing_trajectory_delay_time_offset = gg->get_swing_trajectory_delay_time_offset();
   i_param.gravitational_acceleration = gg->get_gravitational_acceleration();
+  i_param.toe_angle = gg->get_toe_angle();
+  i_param.heel_angle = gg->get_heel_angle();
+  i_param.toe_pos_offset_x = gg->get_toe_pos_offset_x();
+  i_param.heel_pos_offset_x = gg->get_heel_pos_offset_x();
+  double ratio[gg->get_NUM_TH_PHASES()];
+  gg->get_toe_heel_phase_ratio(ratio);
+  for (int i = 0; i < gg->get_NUM_TH_PHASES(); i++) i_param.toe_heel_phase_ratio[i] = ratio[i];
   return true;
 };
 

rtc/AutoBalancer/AutoBalancerService_impl.cpp

@@ -53,6 +53,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);
   return m_autobalancer->getGaitGeneratorParam(*i_param);
 };
 

rtc/AutoBalancer/CMakeLists.txt

@@ -7,7 +7,7 @@ set_target_properties(AutoBalancer PROPERTIES PREFIX "")
 add_executable(testPreviewController testPreviewController.cpp ../ImpedanceController/RatsMatrix.cpp PreviewController.cpp)
 target_link_libraries(testPreviewController ${libs})
 
-add_executable(testGaitGenerator testGaitGenerator.cpp ../ImpedanceController/RatsMatrix.cpp PreviewController.cpp GaitGenerator.cpp)
+add_executable(testGaitGenerator testGaitGenerator.cpp ../ImpedanceController/RatsMatrix.cpp PreviewController.cpp GaitGenerator.cpp ../SequencePlayer/interpolator.cpp)
 target_link_libraries(testGaitGenerator ${libs})
 
 add_executable(AutoBalancerComp AutoBalancerComp.cpp ${comp_sources})

rtc/AutoBalancer/GaitGenerator.cpp

@@ -85,6 +85,9 @@ namespace rats
       break;
     default: break;
     }
+    if (std::fabs(step_height) > 1e-3*10) {
+        modif_foot_coords_for_toe_heel_phase(ret);
+    }
   };
 
   double gait_generator::leg_coords_generator::calc_ratio_from_double_support_ratio (const double default_double_support_ratio, const size_t one_step_len)
@@ -133,6 +136,52 @@ namespace rats
     ret = dvm * cycloid_point + start + uz;
   };
 
+  double gait_generator::leg_coords_generator::calc_interpolated_toe_heel_angle (const toe_heel_phase start_phase, const toe_heel_phase goal_phase, const double start, const double goal)
+  {
+      double tmp_ip_ratio;
+      size_t current_count = total_count - gp_count;
+      if (current_count == toe_heel_phase_count[start_phase]) {
+          toe_heel_interpolator->set(&start);
+          toe_heel_interpolator->go(&goal, _dt * (toe_heel_phase_count[goal_phase]-toe_heel_phase_count[start_phase]));
+      }
+      toe_heel_interpolator->get(&tmp_ip_ratio, true);
+      return tmp_ip_ratio;
+  };
+
+  void gait_generator::leg_coords_generator::modif_foot_coords_for_toe_heel_phase (coordinates& org_coords)
+  {
+      coordinates new_coords;
+      size_t current_count = total_count - gp_count;
+      double dif_angle = 0.0;
+      hrp::Vector3 ee_local_pivot_pos(hrp::Vector3(0,0,0));
+      if ( (toe_heel_phase_count[SOLE0] <= current_count) && (current_count < toe_heel_phase_count[SOLE2TOE]) ) {
+          dif_angle = calc_interpolated_toe_heel_angle(SOLE0, SOLE2TOE, 0.0, toe_angle);
+          ee_local_pivot_pos(0) = toe_pos_offset_x;
+      } else if ( (toe_heel_phase_count[SOLE2HEEL] <= current_count) && (current_count < toe_heel_phase_count[HEEL2SOLE]) ) {
+          dif_angle = calc_interpolated_toe_heel_angle(SOLE2HEEL, HEEL2SOLE, -1 * heel_angle, 0.0);
+          ee_local_pivot_pos(0) = -1 * heel_pos_offset_x;
+      } else if ( (toe_heel_phase_count[SOLE2TOE] <= current_count) && (current_count < toe_heel_phase_count[SOLE2HEEL]) ) {
+          // If SOLE1 phase does not exist, interpolate toe => heel smoothly, without 0 velocity phase.
+          if ( toe_heel_phase_count[TOE2SOLE] == toe_heel_phase_count[SOLE1] ) {
+              dif_angle = calc_interpolated_toe_heel_angle(SOLE2TOE, SOLE2HEEL, toe_angle, -1 * heel_angle);
+              if ( dif_angle > 0) ee_local_pivot_pos(0) = toe_pos_offset_x;
+              else ee_local_pivot_pos(0) = -1 * heel_pos_offset_x;
+          } else {
+              if ( (toe_heel_phase_count[SOLE2TOE] <= current_count) && (current_count < toe_heel_phase_count[TOE2SOLE]) ) {
+                  dif_angle = calc_interpolated_toe_heel_angle(SOLE2TOE, TOE2SOLE, toe_angle, 0.0);
+                  ee_local_pivot_pos(0) = toe_pos_offset_x;
+              } else if ( (toe_heel_phase_count[SOLE1] <= current_count) && (current_count < toe_heel_phase_count[SOLE2HEEL]) ) {
+                  dif_angle = calc_interpolated_toe_heel_angle(SOLE1, SOLE2HEEL, 0.0, -1 * heel_angle);
+                  ee_local_pivot_pos(0) = -1 * heel_pos_offset_x;
+              }
+          }
+      }
+      Eigen::AngleAxis<double> tmpr(deg2rad(dif_angle), hrp::Vector3::UnitY());
+      rotm3times(new_coords.rot, org_coords.rot, tmpr.toRotationMatrix());
+      new_coords.pos = org_coords.pos + org_coords.rot * ee_local_pivot_pos - new_coords.rot * ee_local_pivot_pos;
+      org_coords = new_coords;
+  };
+
   void gait_generator::leg_coords_generator::cycloid_midcoords (coordinates& ret,
                                                                 const double ratio, const coordinates& start,
                                                                 const coordinates& goal, const double height) const
@@ -159,7 +208,7 @@ namespace rats
 
   void gait_generator::leg_coords_generator::update_leg_coords (const std::vector<step_node>& fnl, const double default_double_support_ratio, const size_t one_step_len, const bool force_height_zero)
   {
-    rot_ratio = 1.0 - (double)gp_count / one_step_len;
+    foot_ratio_interpolator->get(&rot_ratio, true);
     if ( 0 == gp_index ) {
       swing_leg_dst_coords = fnl[gp_index].worldcoords;
       support_leg = fnl[gp_index+1].l_r;
@@ -191,6 +240,10 @@ namespace rats
       gp_count = one_step_len;
       rdtg.reset(one_step_len, default_double_support_ratio);
       sdtg.reset(one_step_len, default_double_support_ratio);
+      double tmp_ratio = 0.0;
+      foot_ratio_interpolator->set(&tmp_ratio);
+      tmp_ratio = 1.0;
+      foot_ratio_interpolator->go(&tmp_ratio, _dt*one_step_len, true);
     }
   };
 

rtc/AutoBalancer/GaitGenerator.h

@@ -3,6 +3,7 @@
 #define GAITGENERATOR_H
 #include "PreviewController.h"
 #include "../ImpedanceController/RatsMatrix.h"
+#include "interpolator.h"
 #include <vector>
 #include <queue>
 
@@ -267,15 +268,24 @@ namespace rats
 #ifdef HAVE_MAIN
     public:
 #endif
+      enum toe_heel_phase {SOLE0, SOLE2TOE, TOE2SOLE, SOLE1, SOLE2HEEL, HEEL2SOLE, SOLE2, NUM_TH_PHASES};
       coordinates swing_leg_dst_coords, support_leg_coords, swing_leg_coords, swing_leg_src_coords;
       double default_step_height, default_top_ratio, current_step_height, swing_ratio, rot_ratio, _dt, current_swing_time[2];
-      size_t gp_index, gp_count;
+      size_t gp_index, gp_count, total_count;
       leg_type support_leg;
       orbit_type default_orbit_type;
       rectangle_delay_hoffarbib_trajectory_generator rdtg;
       stair_delay_hoffarbib_trajectory_generator sdtg;
+      interpolator* foot_ratio_interpolator;
+      // Parameters for toe-heel contact
+      interpolator* toe_heel_interpolator;
+      size_t toe_heel_phase_count[NUM_TH_PHASES];
+      double toe_heel_phase_ratio[NUM_TH_PHASES];
+      double toe_pos_offset_x, heel_pos_offset_x, toe_angle, heel_angle;
       void calc_current_swing_leg_coords (coordinates& ret,
                                           const double ratio, const double step_height);
+      double calc_interpolated_toe_heel_angle (const toe_heel_phase start_phase, const toe_heel_phase goal_phase, const double start, const double goal);
+      void modif_foot_coords_for_toe_heel_phase (coordinates& org_coords);
       void cycloid_midcoords (coordinates& ret,
                               const double ratio, const coordinates& start,
                               const coordinates& goal, const double height) const;
@@ -294,12 +304,29 @@ namespace rats
 #endif
       leg_coords_generator(const double __dt)
         : swing_leg_dst_coords(), support_leg_coords(), swing_leg_coords(), swing_leg_src_coords(),
-          default_step_height(0.05), default_top_ratio(0.5), current_step_height(0.0), swing_ratio(0), rot_ratio(0), _dt(__dt), gp_index(0), gp_count(0), support_leg(WC_RLEG), default_orbit_type(CYCLOID)
+          default_step_height(0.05), default_top_ratio(0.5), current_step_height(0.0), swing_ratio(0), rot_ratio(0), _dt(__dt), gp_index(0), gp_count(0), support_leg(WC_RLEG), default_orbit_type(CYCLOID),
+          foot_ratio_interpolator(NULL), toe_heel_interpolator(NULL),
+          toe_pos_offset_x(0.0), heel_pos_offset_x(0.0), toe_angle(0.0), heel_angle(0.0)
       {
         rdtg.set_dt(_dt);
         sdtg.set_dt(_dt);
+        if (foot_ratio_interpolator == NULL) foot_ratio_interpolator = new interpolator(1, __dt);
+        //if (foot_ratio_interpolator == NULL) foot_ratio_interpolator = new interpolator(1, __dt, interpolator::LINEAR);
+        if (toe_heel_interpolator == NULL) toe_heel_interpolator = new interpolator(1, __dt);
+        double ratio[NUM_TH_PHASES] = {0.05,0.25,0.2,0.0,0.2,0.25,0.05};
+        set_toe_heel_phase_ratio(ratio);
+      };
+      ~leg_coords_generator()
+      {
+        if (foot_ratio_interpolator != NULL) {
+            delete foot_ratio_interpolator;
+            foot_ratio_interpolator = NULL;
+        }
+        if (toe_heel_interpolator != NULL) {
+            delete toe_heel_interpolator;
+            toe_heel_interpolator = NULL;
+        }
       };
-      ~leg_coords_generator() {};
       void set_default_step_height (const double _tmp) { default_step_height = _tmp; };
       void set_default_top_ratio (const double _tmp) { default_top_ratio = _tmp; };
       void set_default_orbit_type (const orbit_type _tmp) { default_orbit_type = _tmp; };
@@ -309,6 +336,22 @@ namespace rats
         sdtg.set_swing_trajectory_delay_time_offset(_time_offset);
       };
       void set_stair_trajectory_way_point_offset (const hrp::Vector3 _offset) { sdtg.set_stair_trajectory_way_point_offset(_offset); };
+      void set_toe_pos_offset_x (const double _offx) { toe_pos_offset_x = _offx; };
+      void set_heel_pos_offset_x (const double _offx) { heel_pos_offset_x = _offx; };
+      void set_toe_angle (const double _angle) { toe_angle = _angle; };
+      void set_heel_angle (const double _angle) { heel_angle = _angle; };
+      void set_toe_heel_phase_ratio (const double* ratio)
+      {
+        for (size_t i = 0; i < NUM_TH_PHASES; i++) toe_heel_phase_ratio[i] = ratio[i];
+      };
+      void set_toe_heel_phase_count_from_raio (const double* ratio)
+      {
+        double ratio_sum = 0.0;
+        for (size_t i = 0; i < NUM_TH_PHASES; i++) {
+            ratio_sum += ratio[i];
+            toe_heel_phase_count[i] = static_cast<size_t>(total_count * ratio_sum);
+        }
+      };
       void reset(const size_t one_step_len,
                  const coordinates& _swing_leg_dst_coords,
                  const coordinates& _swing_leg_src_coords,
@@ -318,11 +361,12 @@ namespace rats
         swing_leg_dst_coords = _swing_leg_dst_coords;
         swing_leg_src_coords = _swing_leg_src_coords;
         support_leg_coords = _support_leg_coords;
-        gp_count = one_step_len;
+        total_count = gp_count = one_step_len;
         gp_index = 0;
         current_step_height = 0.0;
         rdtg.reset(one_step_len, default_double_support_ratio);
         sdtg.reset(one_step_len, default_double_support_ratio);
+        set_toe_heel_phase_count_from_raio(toe_heel_phase_ratio);
       };
       void update_leg_coords (const std::vector<step_node>& fnl, const double default_double_support_ratio, const size_t one_step_len, const bool force_height_zero);
       size_t get_gp_index() const { return gp_index; };
@@ -356,6 +400,15 @@ namespace rats
       orbit_type get_default_orbit_type () const { return default_orbit_type; };
       double get_swing_trajectory_delay_time_offset () { return rdtg.get_swing_trajectory_delay_time_offset(); };
       hrp::Vector3 get_stair_trajectory_way_point_offset () { return sdtg.get_stair_trajectory_way_point_offset(); };
+      double get_toe_pos_offset_x () { return toe_pos_offset_x; };
+      double get_heel_pos_offset_x () { return heel_pos_offset_x; };
+      double get_toe_angle () { return toe_angle; };
+      double get_heel_angle () { return heel_angle; };
+      void get_toe_heel_phase_ratio (double* ratio)
+      {
+          for (size_t i = 0; i < NUM_TH_PHASES; i++) ratio[i] = toe_heel_phase_ratio[i];
+      };
+      int get_NUM_TH_PHASES () { return NUM_TH_PHASES; };
     };
 
     enum velocity_mode_flag { VEL_IDLING, VEL_DOING, VEL_ENDING };
@@ -479,6 +532,11 @@ namespace rats
     void set_swing_trajectory_delay_time_offset (const double _time_offset) { lcg.set_swing_trajectory_delay_time_offset(_time_offset); };
     void set_stair_trajectory_way_point_offset (const hrp::Vector3 _offset) { lcg.set_stair_trajectory_way_point_offset(_offset); };
     void set_gravitational_acceleration (const double ga) { gravitational_acceleration = ga; };
+    void set_toe_pos_offset_x (const double _offx) { lcg.set_toe_pos_offset_x(_offx); };
+    void set_heel_pos_offset_x (const double _offx) { lcg.set_heel_pos_offset_x(_offx); };
+    void set_toe_angle (const double _angle) { lcg.set_toe_angle(_angle); };
+    void set_heel_angle (const double _angle) { lcg.set_heel_angle(_angle); };
+    void set_toe_heel_phase_ratio (const double* ratio) { lcg.set_toe_heel_phase_ratio(ratio); };
     void print_footstep_list () const
     {
       for (size_t i = 0; i < footstep_node_list.size(); i++)
@@ -532,6 +590,12 @@ namespace rats
     double get_swing_trajectory_delay_time_offset () { return lcg.get_swing_trajectory_delay_time_offset(); };
     hrp::Vector3 get_stair_trajectory_way_point_offset () { return lcg.get_stair_trajectory_way_point_offset(); };
     double get_gravitational_acceleration () { return gravitational_acceleration; } ;
+    double get_toe_pos_offset_x () { return lcg.get_toe_pos_offset_x(); };
+    double get_heel_pos_offset_x () { return lcg.get_heel_pos_offset_x(); };
+    double get_toe_angle () { return lcg.get_toe_angle(); };
+    double get_heel_angle () { return lcg.get_heel_angle(); };
+    void get_toe_heel_phase_ratio (double* ratio) { lcg.get_toe_heel_phase_ratio(ratio); };
+    int get_NUM_TH_PHASES () { return lcg.get_NUM_TH_PHASES(); };
   };
 }
 #endif /* GAITGENERATOR_H */