From d9ffdbad67bd072acdc7f897f4dd0322e7e40821 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Fri, 3 Apr 2015 11:48:23 +0900 Subject: [PATCH 1/4] Use interpolator for rot_ratio --- rtc/AutoBalancer/CMakeLists.txt | 2 +- rtc/AutoBalancer/GaitGenerator.cpp | 6 +++++- rtc/AutoBalancer/GaitGenerator.h | 11 +++++++++-- 3 files changed, 15 insertions(+), 4 deletions(-) diff --git a/rtc/AutoBalancer/CMakeLists.txt b/rtc/AutoBalancer/CMakeLists.txt index 5e5c0f13a75..9f275925bbd 100644 --- a/rtc/AutoBalancer/CMakeLists.txt +++ b/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}) diff --git a/rtc/AutoBalancer/GaitGenerator.cpp b/rtc/AutoBalancer/GaitGenerator.cpp index b2d63e921a6..2d1e467bed3 100644 --- a/rtc/AutoBalancer/GaitGenerator.cpp +++ b/rtc/AutoBalancer/GaitGenerator.cpp @@ -159,7 +159,7 @@ namespace rats void gait_generator::leg_coords_generator::update_leg_coords (const std::vector& 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 +191,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); } }; diff --git a/rtc/AutoBalancer/GaitGenerator.h b/rtc/AutoBalancer/GaitGenerator.h index 12c2a0e0556..ef20a8ba936 100644 --- a/rtc/AutoBalancer/GaitGenerator.h +++ b/rtc/AutoBalancer/GaitGenerator.h @@ -3,6 +3,7 @@ #define GAITGENERATOR_H #include "PreviewController.h" #include "../ImpedanceController/RatsMatrix.h" +#include "interpolator.h" #include #include @@ -274,6 +275,7 @@ namespace rats orbit_type default_orbit_type; rectangle_delay_hoffarbib_trajectory_generator rdtg; stair_delay_hoffarbib_trajectory_generator sdtg; + interpolator *foot_ratio_interpolator; void calc_current_swing_leg_coords (coordinates& ret, const double ratio, const double step_height); void cycloid_midcoords (coordinates& ret, @@ -294,12 +296,17 @@ 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(new interpolator(1, __dt, interpolator::LINEAR)) + foot_ratio_interpolator(new interpolator(1, __dt)) { rdtg.set_dt(_dt); sdtg.set_dt(_dt); }; - ~leg_coords_generator() {}; + ~leg_coords_generator() + { + delete foot_ratio_interpolator; + }; 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; }; From 8989c23cd86095d9fe830f56d3d042ead9f89dd8 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Sat, 4 Apr 2015 01:00:51 +0900 Subject: [PATCH 2/4] Add toe-off and heel-contact parameters. --- idl/AutoBalancerService.idl | 8 +++++ rtc/AutoBalancer/AutoBalancer.cpp | 13 ++++++++ rtc/AutoBalancer/GaitGenerator.cpp | 45 +++++++++++++++++++++++++++ rtc/AutoBalancer/GaitGenerator.h | 50 ++++++++++++++++++++++++++---- 4 files changed, 110 insertions(+), 6 deletions(-) diff --git a/idl/AutoBalancerService.idl b/idl/AutoBalancerService.idl index 00b04d6a2d0..3f98492fb26 100644 --- a/idl/AutoBalancerService.idl +++ b/idl/AutoBalancerService.idl @@ -115,6 +115,14 @@ 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; }; /** diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp index 3d6d3cffcf0..ff1a899435a 100644 --- a/rtc/AutoBalancer/AutoBalancer.cpp +++ b/rtc/AutoBalancer/AutoBalancer.cpp @@ -992,6 +992,11 @@ 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); + // 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 +1020,10 @@ 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; return true; }; @@ -1037,6 +1046,10 @@ 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(); return true; }; diff --git a/rtc/AutoBalancer/GaitGenerator.cpp b/rtc/AutoBalancer/GaitGenerator.cpp index 2d1e467bed3..cdd8bd288ad 100644 --- a/rtc/AutoBalancer/GaitGenerator.cpp +++ b/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,48 @@ namespace rats ret = dvm * cycloid_point + start + uz; }; + double gait_generator::leg_coords_generator::calc_current_toe_heel_ratio (const toe_heel_phase phase) + { + double tmp_ip_ratio; + size_t current_count = total_count - gp_count; + if (current_count == toe_heel_phase_count[phase-1]) { + tmp_ip_ratio = 0.0; + toe_heel_interpolator->set(&tmp_ip_ratio); + tmp_ip_ratio = 1.0; + toe_heel_interpolator->go(&tmp_ip_ratio, _dt * (toe_heel_phase_count[phase]-toe_heel_phase_count[phase-1])); + } + 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) + { + double dif_angle = 0.0; + coordinates new_coords; + size_t current_count = total_count - gp_count; + hrp::Vector3 ee_local_pivot_pos(hrp::Vector3(0,0,0)); + if (current_count < toe_heel_phase_count[SOLE0]) { + } else if (current_count < toe_heel_phase_count[SOLE2TOE]) { + dif_angle = toe_angle * calc_current_toe_heel_ratio(SOLE2TOE); + ee_local_pivot_pos(0) = toe_pos_offset_x; + } else if (current_count < toe_heel_phase_count[TOE2SOLE]) { + dif_angle = toe_angle * (1-calc_current_toe_heel_ratio(TOE2SOLE)); + ee_local_pivot_pos(0) = toe_pos_offset_x; + } else if (current_count < toe_heel_phase_count[SOLE1]) { + } else if (current_count < toe_heel_phase_count[SOLE2HEEL]) { + dif_angle = heel_angle * calc_current_toe_heel_ratio(SOLE2HEEL); + ee_local_pivot_pos(0) = heel_pos_offset_x; + } else if (current_count < toe_heel_phase_count[HEEL2SOLE]) { + dif_angle = heel_angle * (1-calc_current_toe_heel_ratio(HEEL2SOLE)); + ee_local_pivot_pos(0) = heel_pos_offset_x; + } else { // if (current_count < toe_heel_phase_count[SOLE2]) + } + Eigen::AngleAxis 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 diff --git a/rtc/AutoBalancer/GaitGenerator.h b/rtc/AutoBalancer/GaitGenerator.h index ef20a8ba936..9b2816b376d 100644 --- a/rtc/AutoBalancer/GaitGenerator.h +++ b/rtc/AutoBalancer/GaitGenerator.h @@ -268,16 +268,23 @@ 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; + size_t toe_heel_phase_count[NUM_TH_PHASES]; 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; + interpolator* foot_ratio_interpolator; + // Parameters for toe-heel contact + interpolator* toe_heel_interpolator; + 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_current_toe_heel_ratio (const toe_heel_phase phase); + 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; @@ -297,15 +304,25 @@ namespace rats 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), - //foot_ratio_interpolator(new interpolator(1, __dt, interpolator::LINEAR)) - foot_ratio_interpolator(new interpolator(1, __dt)) + 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); }; ~leg_coords_generator() { - delete foot_ratio_interpolator; + 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; + } }; void set_default_step_height (const double _tmp) { default_step_height = _tmp; }; void set_default_top_ratio (const double _tmp) { default_top_ratio = _tmp; }; @@ -316,6 +333,10 @@ 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 reset(const size_t one_step_len, const coordinates& _swing_leg_dst_coords, const coordinates& _swing_leg_src_coords, @@ -325,11 +346,16 @@ 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); + double ratio[NUM_TH_PHASES] = {0.05,0.25,0.2,0.0,0.2,0.25,0.05}, 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(one_step_len * ratio_sum); + } }; void update_leg_coords (const std::vector& 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; }; @@ -363,6 +389,10 @@ 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; }; }; enum velocity_mode_flag { VEL_IDLING, VEL_DOING, VEL_ENDING }; @@ -486,6 +516,10 @@ 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 print_footstep_list () const { for (size_t i = 0; i < footstep_node_list.size(); i++) @@ -539,6 +573,10 @@ 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(); }; }; } #endif /* GAITGENERATOR_H */ From f7a733c906697fc2a643980bc44f7210dd92ab2b Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Sat, 4 Apr 2015 03:17:49 +0900 Subject: [PATCH 3/4] Enable to interpolate smoothly if SOLE1 phase does not exist --- rtc/AutoBalancer/GaitGenerator.cpp | 46 ++++++++++++++++-------------- rtc/AutoBalancer/GaitGenerator.h | 2 +- 2 files changed, 26 insertions(+), 22 deletions(-) diff --git a/rtc/AutoBalancer/GaitGenerator.cpp b/rtc/AutoBalancer/GaitGenerator.cpp index cdd8bd288ad..db8aaffe537 100644 --- a/rtc/AutoBalancer/GaitGenerator.cpp +++ b/rtc/AutoBalancer/GaitGenerator.cpp @@ -136,15 +136,13 @@ namespace rats ret = dvm * cycloid_point + start + uz; }; - double gait_generator::leg_coords_generator::calc_current_toe_heel_ratio (const toe_heel_phase phase) + 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[phase-1]) { - tmp_ip_ratio = 0.0; - toe_heel_interpolator->set(&tmp_ip_ratio); - tmp_ip_ratio = 1.0; - toe_heel_interpolator->go(&tmp_ip_ratio, _dt * (toe_heel_phase_count[phase]-toe_heel_phase_count[phase-1])); + 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; @@ -152,25 +150,31 @@ namespace rats void gait_generator::leg_coords_generator::modif_foot_coords_for_toe_heel_phase (coordinates& org_coords) { - double dif_angle = 0.0; 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 (current_count < toe_heel_phase_count[SOLE0]) { - } else if (current_count < toe_heel_phase_count[SOLE2TOE]) { - dif_angle = toe_angle * calc_current_toe_heel_ratio(SOLE2TOE); - ee_local_pivot_pos(0) = toe_pos_offset_x; - } else if (current_count < toe_heel_phase_count[TOE2SOLE]) { - dif_angle = toe_angle * (1-calc_current_toe_heel_ratio(TOE2SOLE)); + 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 (current_count < toe_heel_phase_count[SOLE1]) { - } else if (current_count < toe_heel_phase_count[SOLE2HEEL]) { - dif_angle = heel_angle * calc_current_toe_heel_ratio(SOLE2HEEL); - ee_local_pivot_pos(0) = heel_pos_offset_x; - } else if (current_count < toe_heel_phase_count[HEEL2SOLE]) { - dif_angle = heel_angle * (1-calc_current_toe_heel_ratio(HEEL2SOLE)); - ee_local_pivot_pos(0) = heel_pos_offset_x; - } else { // if (current_count < toe_heel_phase_count[SOLE2]) + } 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 tmpr(deg2rad(dif_angle), hrp::Vector3::UnitY()); rotm3times(new_coords.rot, org_coords.rot, tmpr.toRotationMatrix()); diff --git a/rtc/AutoBalancer/GaitGenerator.h b/rtc/AutoBalancer/GaitGenerator.h index 9b2816b376d..305edefa552 100644 --- a/rtc/AutoBalancer/GaitGenerator.h +++ b/rtc/AutoBalancer/GaitGenerator.h @@ -283,7 +283,7 @@ namespace rats 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_current_toe_heel_ratio (const toe_heel_phase phase); + 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, From f9d4f35f81af6c1539ee56fa951f3fe7eeb85693 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Sat, 4 Apr 2015 03:58:22 +0900 Subject: [PATCH 4/4] Enable to set phase time ratio --- idl/AutoBalancerService.idl | 2 ++ rtc/AutoBalancer/AutoBalancer.cpp | 17 ++++++++++ rtc/AutoBalancer/AutoBalancerService_impl.cpp | 1 + rtc/AutoBalancer/GaitGenerator.h | 31 +++++++++++++++---- 4 files changed, 45 insertions(+), 6 deletions(-) diff --git a/idl/AutoBalancerService.idl b/idl/AutoBalancerService.idl index 3f98492fb26..06ac0556423 100644 --- a/idl/AutoBalancerService.idl +++ b/idl/AutoBalancerService.idl @@ -123,6 +123,8 @@ module OpenHRP 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 toe_heel_phase_ratio; }; /** diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp index ff1a899435a..db4c9edd875 100644 --- a/rtc/AutoBalancer/AutoBalancer.cpp +++ b/rtc/AutoBalancer/AutoBalancer.cpp @@ -996,6 +996,11 @@ bool AutoBalancer::setGaitGeneratorParam(const OpenHRP::AutoBalancerService::Gai 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; @@ -1024,6 +1029,15 @@ bool AutoBalancer::setGaitGeneratorParam(const OpenHRP::AutoBalancerService::Gai 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; }; @@ -1050,6 +1064,9 @@ bool AutoBalancer::getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGener 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; }; diff --git a/rtc/AutoBalancer/AutoBalancerService_impl.cpp b/rtc/AutoBalancer/AutoBalancerService_impl.cpp index 7ad4cfea007..813d86d81ca 100644 --- a/rtc/AutoBalancer/AutoBalancerService_impl.cpp +++ b/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); }; diff --git a/rtc/AutoBalancer/GaitGenerator.h b/rtc/AutoBalancer/GaitGenerator.h index 305edefa552..ff9a38fe5d2 100644 --- a/rtc/AutoBalancer/GaitGenerator.h +++ b/rtc/AutoBalancer/GaitGenerator.h @@ -272,7 +272,6 @@ namespace rats 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, total_count; - size_t toe_heel_phase_count[NUM_TH_PHASES]; leg_type support_leg; orbit_type default_orbit_type; rectangle_delay_hoffarbib_trajectory_generator rdtg; @@ -280,6 +279,8 @@ namespace rats 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); @@ -312,6 +313,8 @@ namespace rats 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() { @@ -337,6 +340,18 @@ namespace rats 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(total_count * ratio_sum); + } + }; void reset(const size_t one_step_len, const coordinates& _swing_leg_dst_coords, const coordinates& _swing_leg_src_coords, @@ -351,11 +366,7 @@ namespace rats current_step_height = 0.0; rdtg.reset(one_step_len, default_double_support_ratio); sdtg.reset(one_step_len, default_double_support_ratio); - double ratio[NUM_TH_PHASES] = {0.05,0.25,0.2,0.0,0.2,0.25,0.05}, 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(one_step_len * ratio_sum); - } + set_toe_heel_phase_count_from_raio(toe_heel_phase_ratio); }; void update_leg_coords (const std::vector& 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; }; @@ -393,6 +404,11 @@ namespace rats 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 }; @@ -520,6 +536,7 @@ namespace rats 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++) @@ -577,6 +594,8 @@ namespace rats 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 */