Skip to content

Commit

Permalink
[rtc/AutoBalancer/*GaitGenerator*] Use round+cast instead of cast to …
Browse files Browse the repository at this point in the history 
…avoid the problem fkanehiro#1230
  • Loading branch information
@snozawa
snozawa committed Feb 19, 2018
1 parent cecb9fb commit bd51537
Show file tree
Hide file tree
Showing 3 changed files with 22 additions and 22 deletions.
26 changes: 13 additions & 13 deletions rtc/AutoBalancer/GaitGenerator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -98,7 +98,7 @@ namespace rats
swing_leg_types.push_back(fns.at(i).l_r);
}
swing_leg_types_list.push_back( swing_leg_types );
step_count_list.push_back(static_cast<size_t>(fns.front().step_time/dt));
step_count_list.push_back(static_cast<size_t>(round(fns.front().step_time/dt)));
toe_heel_types_list.push_back(toe_heel_types(SOLE, SOLE));
//std::cerr << "double " << (fns[fs_index].l_r==RLEG?LLEG:RLEG) << " [" << refzmp_cur_list.back()(0) << " " << refzmp_cur_list.back()(1) << " " << refzmp_cur_list.back()(2) << "]" << std::endl;
};
Expand All @@ -125,7 +125,7 @@ namespace rats
swing_leg_types.push_back(fns.at(i).l_r);
}
swing_leg_types_list.push_back( swing_leg_types );
step_count_list.push_back(static_cast<size_t>(fns.front().step_time/dt));
step_count_list.push_back(static_cast<size_t>(round(fns.front().step_time/dt)));
toe_heel_types_list.push_back(tht);
//std::cerr << "single " << fns[fs_index-1].l_r << " [" << refzmp_cur_list.back()(0) << " " << refzmp_cur_list.back()(1) << " " << refzmp_cur_list.back()(2) << "]" << std::endl;
};
Expand Down Expand Up @@ -564,11 +564,11 @@ namespace rats
current_step_height = current_toe_angle = current_heel_angle = 0.0;
}
if (footstep_index < fnsl.size()) {
one_step_count = static_cast<size_t>(fnsl[footstep_index].front().step_time/dt);
one_step_count = static_cast<size_t>(round(fnsl[footstep_index].front().step_time/dt));
thp.set_one_step_count(one_step_count);
}
if (footstep_index + 1 < fnsl.size()) {
next_one_step_count = static_cast<size_t>(fnsl[footstep_index+1].front().step_time/dt);
next_one_step_count = static_cast<size_t>(round(fnsl[footstep_index+1].front().step_time/dt));
}
lcg_count = one_step_count;
switch (default_orbit_type) {
Expand Down Expand Up @@ -602,7 +602,7 @@ namespace rats
const double delay)
{
/* clear all gait_parameter */
size_t one_step_len = footstep_nodes_list.front().front().step_time / dt;
size_t one_step_len = static_cast<size_t>(round(footstep_nodes_list.front().front().step_time / dt));
finalize_count = 0;
for (std::vector<step_node>::iterator it_fns = footstep_nodes_list.front().begin(); it_fns != footstep_nodes_list.front().end(); it_fns++) {
for (std::vector<step_node>::const_iterator it_init = initial_swing_leg_dst_steps.begin(); it_init != initial_swing_leg_dst_steps.end(); it_init++) {
Expand Down Expand Up @@ -631,7 +631,7 @@ namespace rats
}
//preview_controller_ptr = new preview_dynamics_filter<preview_control>(dt, cog(2) - refzmp_cur_list[0](2), refzmp_cur_list[0]);
preview_controller_ptr = new preview_dynamics_filter<extended_preview_control>(dt, cog(2) - rg.get_refzmp_cur()(2), rg.get_refzmp_cur(), gravitational_acceleration);
lcg.reset(one_step_len, footstep_nodes_list.at(1).front().step_time/dt, initial_swing_leg_dst_steps, initial_swing_leg_dst_steps, initial_support_leg_steps, default_double_support_ratio_swing_before, default_double_support_ratio_swing_after);
lcg.reset(one_step_len, static_cast<size_t>(round(footstep_nodes_list.at(1).front().step_time/dt)), initial_swing_leg_dst_steps, initial_swing_leg_dst_steps, initial_support_leg_steps, default_double_support_ratio_swing_before, default_double_support_ratio_swing_after);
/* make another */
lcg.set_swing_support_steps_list(footstep_nodes_list);
for (size_t i = 1; i < footstep_nodes_list.size()-1; i++) {
Expand Down Expand Up @@ -722,7 +722,7 @@ namespace rats
prev_que_rzmp = rzmp;
prev_que_sfzos = sfzos;
}
solved = preview_controller_ptr->update(refzmp, cog, swing_foot_zmp_offsets, rzmp, sfzos, (refzmp_exist_p || finalize_count < preview_controller_ptr->get_delay()-default_step_time/dt));
solved = preview_controller_ptr->update(refzmp, cog, swing_foot_zmp_offsets, rzmp, sfzos, (refzmp_exist_p || finalize_count < preview_controller_ptr->get_delay()-static_cast<size_t>(round(default_step_time/dt))));
}

rg.update_refzmp();
Expand Down Expand Up @@ -794,7 +794,7 @@ namespace rats
if (lcg.get_footstep_index() > 0 && lcg.get_footstep_index() < footstep_nodes_list.size()-2) {
// calculate sum of preview_f
static double preview_f_sum;
if (lcg.get_lcg_count() == static_cast<size_t>(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * 1.0) - 1) {
if (lcg.get_lcg_count() == static_cast<size_t>(round(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * 1.0)) - 1) {
preview_f_sum = preview_controller_ptr->get_preview_f(preview_controller_ptr->get_delay());
for (size_t i = preview_controller_ptr->get_delay()-1; i >= lcg.get_lcg_count()+1; i--) {
preview_f_sum += preview_controller_ptr->get_preview_f(i);
Expand All @@ -809,9 +809,9 @@ namespace rats
hrp::Vector3 d_footstep = -1/preview_f_sum * 1/preview_db * footstep_modification_gain * tmp_diff_cp;
d_footstep(2) = 0.0;
// overwrite footsteps
if (lcg.get_lcg_count() <= static_cast<size_t>(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * 1.0) - 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 + margin_time_ratio)) - 1 &&
!(lcg.get_lcg_count() <= static_cast<size_t>(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * 0.5) - 1 && act_contact_states[0] && act_contact_states[1])) {
if (lcg.get_lcg_count() <= static_cast<size_t>(round(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * 1.0)) - 1 &&
lcg.get_lcg_count() >= static_cast<size_t>(round(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * (default_double_support_ratio_after + margin_time_ratio))) - 1 &&
!(lcg.get_lcg_count() <= static_cast<size_t>(round(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * 0.5)) - 1 && act_contact_states[0] && act_contact_states[1])) {
// stride limitation check
hrp::Vector3 orig_footstep_pos = footstep_nodes_list[get_overwritable_index()].front().worldcoords.pos;
for (size_t i = 0; i < 2; i++) {
Expand Down Expand Up @@ -1100,7 +1100,7 @@ namespace rats
if (overwritable_footstep_index_offset == 0) {
rg.set_refzmp_count(lcg.get_lcg_count()); // Start refzmp_count from current remaining footstep count of swinging.
} else {
rg.set_refzmp_count(static_cast<size_t>(fnsl[0][0].step_time/dt)); // Start refzmp_count from step length of first overwrite step
rg.set_refzmp_count(static_cast<size_t>(round(fnsl[0][0].step_time/dt))); // Start refzmp_count from step length of first overwrite step
}
/* reset refzmp */
for (size_t i = 0; i < fnsl.size(); i++) {
Expand Down Expand Up @@ -1154,7 +1154,7 @@ namespace rats
prev_que_rzmp = rzmp;
prev_que_sfzos = sfzos;
}
solved = preview_controller_ptr->update(refzmp, cog, swing_foot_zmp_offsets, rzmp, sfzos, (refzmp_exist_p || finalize_count < preview_controller_ptr->get_delay()-default_step_time/dt));
solved = preview_controller_ptr->update(refzmp, cog, swing_foot_zmp_offsets, rzmp, sfzos, (refzmp_exist_p || finalize_count < preview_controller_ptr->get_delay()-static_cast<size_t>(round(default_step_time/dt))));
rg.update_refzmp();
};

Expand Down
6 changes: 3 additions & 3 deletions rtc/AutoBalancer/GaitGenerator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -131,7 +131,7 @@ namespace rats
double ratio_sum = 0.0;
for (size_t i = 0; i < NUM_TH_PHASES; i++) {
ratio_sum += toe_heel_phase_ratio[i];
toe_heel_phase_count[i] = static_cast<size_t>(one_step_count * ratio_sum);
toe_heel_phase_count[i] = static_cast<size_t>(round(one_step_count * ratio_sum));
}
};
public:
Expand Down Expand Up @@ -461,7 +461,7 @@ namespace rats
size_t tmp_time_offset_count = time_offset/dt;
//size_t final_path_count = 0; // Revert to previous version
size_t final_path_count = final_path_distance_ratio * swing_one_step_count;
if (final_path_count>static_cast<size_t>(time_offset_xy2z/dt)) final_path_count = static_cast<size_t>(time_offset_xy2z/dt);
if (final_path_count>static_cast<size_t>(round(time_offset_xy2z/dt))) final_path_count = static_cast<size_t>(round(time_offset_xy2z/dt));
// XY interpolation
if (swing_remain_count > final_path_count+tmp_time_offset_count) { // antecedent path is still interpolating
hrp::Vector3 tmpgoal = interpolate_antecedent_path((swing_one_step_count - swing_remain_count) / static_cast<double>(swing_one_step_count - (final_path_count+tmp_time_offset_count)));
Expand Down Expand Up @@ -1499,7 +1499,7 @@ namespace rats
const std::map<leg_type, std::string> get_leg_type_map () const { return leg_type_map; };
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
size_t get_overwrite_check_timing () const { return static_cast<size_t>(round(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * 0.5)) - 1;}; // Almost middle of step time
double get_leg_margin (const size_t idx) const { return leg_margin[idx]; };
double get_stride_limitation_for_circle_type (const size_t idx) const { return stride_limitation_for_circle_type[idx]; };
double get_overwritable_stride_limitation (const size_t idx) const { return overwritable_stride_limitation[idx]; };
Expand Down
12 changes: 6 additions & 6 deletions rtc/AutoBalancer/testGaitGenerator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1184,7 +1184,7 @@ class testGaitGenerator
while ( gg->proc_one_tick() ) {
proc_one_walking_motion(i);
i++;
if ( i > static_cast<size_t>(gg->get_default_step_time()/dt) && gg->get_overwrite_check_timing() ) {
if ( i > static_cast<size_t>(round(gg->get_default_step_time()/dt)) && gg->get_overwrite_check_timing() ) {
gg->finalize_velocity_mode();
}
}
Expand Down Expand Up @@ -1214,15 +1214,15 @@ class testGaitGenerator
while ( gg->proc_one_tick() ) {
proc_one_walking_motion(i);
i++;
if ( i > static_cast<size_t>(gg->get_default_step_time()/dt)*5 && gg->get_overwrite_check_timing() ) {
if ( i > static_cast<size_t>(round(gg->get_default_step_time()/dt))*5 && gg->get_overwrite_check_timing() ) {
gg->finalize_velocity_mode();
} else if ( i > static_cast<size_t>(gg->get_default_step_time()/dt)*4 && gg->get_overwrite_check_timing() ) {
} else if ( i > static_cast<size_t>(round(gg->get_default_step_time()/dt))*4 && gg->get_overwrite_check_timing() ) {
gg->set_velocity_param(0, 0, 0);
} else if ( i > static_cast<size_t>(gg->get_default_step_time()/dt)*3 && gg->get_overwrite_check_timing() ) {
} else if ( i > static_cast<size_t>(round(gg->get_default_step_time()/dt))*3 && gg->get_overwrite_check_timing() ) {
gg->set_velocity_param(0, 0, 10);
} else if ( i > static_cast<size_t>(gg->get_default_step_time()/dt)*2 && gg->get_overwrite_check_timing() ) {
} else if ( i > static_cast<size_t>(round(gg->get_default_step_time()/dt))*2 && gg->get_overwrite_check_timing() ) {
gg->set_velocity_param(0, 0, 0);
} else if ( i > static_cast<size_t>(gg->get_default_step_time()/dt) && gg->get_overwrite_check_timing() ) {
} else if ( i > static_cast<size_t>(round(gg->get_default_step_time()/dt)) && gg->get_overwrite_check_timing() ) {
gg->set_velocity_param(0.1, 0.05, 0);
}
}
Expand Down

0 comments on commit bd51537

Please sign in to comment.