Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add toe-off and heel-contact #553

Merged
merged 5 commits into from
Apr 4, 2015
Merged

Add toe-off and heel-contact #553

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
d9ffdba
Use interpolator for rot_ratio
snozawa Apr 3, 2015
42ca197
Merge branch 'master' of https://github.com/fkanehiro/hrpsys-base int…
snozawa Apr 3, 2015
8989c23
Add toe-off and heel-contact parameters.
snozawa Apr 3, 2015
f7a733c
Enable to interpolate smoothly if SOLE1 phase does not exist
snozawa Apr 3, 2015
f9d4f35
Enable to set phase time ratio
snozawa Apr 3, 2015
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 10 additions & 0 deletions idl/AutoBalancerService.idl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
};

/**
Expand Down
30 changes: 30 additions & 0 deletions rtc/AutoBalancer/AutoBalancer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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;
};

Expand All @@ -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;
};

Expand Down
1 change: 1 addition & 0 deletions rtc/AutoBalancer/AutoBalancerService_impl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
};

Expand Down
2 changes: 1 addition & 1 deletion rtc/AutoBalancer/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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})
Expand Down
55 changes: 54 additions & 1 deletion rtc/AutoBalancer/GaitGenerator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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
Expand All @@ -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;
Expand Down Expand Up @@ -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);
}
};

Expand Down
72 changes: 68 additions & 4 deletions rtc/AutoBalancer/GaitGenerator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
#define GAITGENERATOR_H
#include "PreviewController.h"
#include "../ImpedanceController/RatsMatrix.h"
#include "interpolator.h"
#include <vector>
#include <queue>

Expand Down Expand Up @@ -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;
Expand All @@ -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; };
Expand All @@ -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,
Expand All @@ -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; };
Expand Down Expand Up @@ -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 };
Expand Down Expand Up @@ -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++)
Expand Down Expand Up @@ -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 */
Expand Down