Skip to content

Commit

Permalink
[testGaitGenerator.cpp] cannot use comparison operator between const …
Browse files Browse the repository at this point in the history 
…std::vector<std::string> and boost::assign::list_of(std::string) in HRP2 inside PC
  • Loading branch information
@eisoku9618
eisoku9618 committed Aug 27, 2015
1 parent eead4fb commit a3a260e
Showing 1 changed file with 17 additions and 8 deletions.
25 changes: 17 additions & 8 deletions rtc/AutoBalancer/testGaitGenerator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@ class testGaitGenerator
hrp::Vector3 min_rfoot_pos(1e10,1e10,1e10), min_lfoot_pos(1e10,1e10,1e10), max_rfoot_pos(-1e10,-1e10,-1e10), max_lfoot_pos(-1e10,-1e10,-1e10);
//
hrp::Vector3 prev_refzmp;
std::vector<std::string> tmp_string_vector;
while ( gg->proc_one_tick() ) {
//std::cerr << gg->lcg.gp_count << std::endl;
// if ( gg->lcg.gp_index == 4 && gg->lcg.gp_count == 100) {
Expand All @@ -69,36 +70,42 @@ class testGaitGenerator
fprintf(fp, "%f ", cogpos);
}
// Foot pos
hrp::Vector3 rfoot_pos = (gg->get_support_leg_names() == boost::assign::list_of("rleg")) ? gg->get_support_leg_steps().front().worldcoords.pos : gg->get_swing_leg_steps().front().worldcoords.pos;
tmp_string_vector = boost::assign::list_of("rleg");
hrp::Vector3 rfoot_pos = (gg->get_support_leg_names() == tmp_string_vector) ? gg->get_support_leg_steps().front().worldcoords.pos : gg->get_swing_leg_steps().front().worldcoords.pos;
for (size_t ii = 0; ii < 3; ii++) {
fprintf(fp, "%f ", rfoot_pos(ii));
min_rfoot_pos(ii) = std::min(min_rfoot_pos(ii), rfoot_pos(ii));
max_rfoot_pos(ii) = std::max(max_rfoot_pos(ii), rfoot_pos(ii));
}
hrp::Vector3 lfoot_pos = (gg->get_support_leg_names() == boost::assign::list_of("lleg")) ? gg->get_support_leg_steps().front().worldcoords.pos : gg->get_swing_leg_steps().front().worldcoords.pos;
tmp_string_vector = boost::assign::list_of("lleg");
hrp::Vector3 lfoot_pos = (gg->get_support_leg_names() == tmp_string_vector) ? gg->get_support_leg_steps().front().worldcoords.pos : gg->get_swing_leg_steps().front().worldcoords.pos;
for (size_t ii = 0; ii < 3; ii++) {
fprintf(fp, "%f ", lfoot_pos(ii));
min_lfoot_pos(ii) = std::min(min_lfoot_pos(ii), lfoot_pos(ii));
max_lfoot_pos(ii) = std::max(max_lfoot_pos(ii), lfoot_pos(ii));
}
// Foot rot
hrp::Vector3 rpy;
hrp::Matrix33 rfoot_rot = (gg->get_support_leg_names() == boost::assign::list_of("rleg")) ? gg->get_support_leg_steps().front().worldcoords.rot : gg->get_swing_leg_steps().front().worldcoords.rot;
tmp_string_vector = boost::assign::list_of("rleg");
hrp::Matrix33 rfoot_rot = (gg->get_support_leg_names() == tmp_string_vector) ? gg->get_support_leg_steps().front().worldcoords.rot : gg->get_swing_leg_steps().front().worldcoords.rot;
rpy = hrp::rpyFromRot(rfoot_rot);
for (size_t ii = 0; ii < 3; ii++) {
fprintf(fp, "%f ", 180.0*rpy(ii)/M_PI);
}
hrp::Matrix33 lfoot_rot = (gg->get_support_leg_names() == boost::assign::list_of("lleg")) ? gg->get_support_leg_steps().front().worldcoords.rot : gg->get_swing_leg_steps().front().worldcoords.rot;
tmp_string_vector = boost::assign::list_of("lleg");
hrp::Matrix33 lfoot_rot = (gg->get_support_leg_names() == tmp_string_vector) ? gg->get_support_leg_steps().front().worldcoords.rot : gg->get_swing_leg_steps().front().worldcoords.rot;
rpy = hrp::rpyFromRot(lfoot_rot);
for (size_t ii = 0; ii < 3; ii++) {
fprintf(fp, "%f ", 180.0*rpy(ii)/M_PI);
}
// ZMP offsets
tmp_string_vector = boost::assign::list_of("rleg");
for (size_t ii = 0; ii < 3; ii++) {
fprintf(fp, "%f ", (gg->get_support_leg_names() == boost::assign::list_of("rleg")) ? gg->get_support_foot_zmp_offsets().front()(ii) : gg->get_swing_foot_zmp_offsets().front()(ii));
fprintf(fp, "%f ", (gg->get_support_leg_names() == tmp_string_vector) ? gg->get_support_foot_zmp_offsets().front()(ii) : gg->get_swing_foot_zmp_offsets().front()(ii));
}
tmp_string_vector = boost::assign::list_of("lleg");
for (size_t ii = 0; ii < 3; ii++) {
fprintf(fp, "%f ", (gg->get_support_leg_names() == boost::assign::list_of("lleg")) ? gg->get_support_foot_zmp_offsets().front()(ii) : gg->get_swing_foot_zmp_offsets().front()(ii));
fprintf(fp, "%f ", (gg->get_support_leg_names() == tmp_string_vector) ? gg->get_support_foot_zmp_offsets().front()(ii) : gg->get_swing_foot_zmp_offsets().front()(ii));
}
// Swing time
fprintf(fp, "%f %f ",
Expand Down Expand Up @@ -326,7 +333,8 @@ class testGaitGenerator

void gen_and_plot_walk_pattern()
{
if (gg->get_footstep_front_leg_names() == boost::assign::list_of("rleg")) {
std::vector<std::string> tmp_string_vector = boost::assign::list_of("rleg");
if (gg->get_footstep_front_leg_names() == tmp_string_vector) {
step_node initial_support_leg_step = step_node(LLEG, coordinates(leg_pos[1]), 0, 0, 0, 0);
step_node initial_swing_leg_dst_step = step_node(RLEG, coordinates(leg_pos[0]), 0, 0, 0, 0);
gen_and_plot_walk_pattern(initial_support_leg_step, initial_swing_leg_dst_step);
Expand Down Expand Up @@ -484,7 +492,8 @@ class testGaitGenerator
mid_coords(start_ref_coords, 0.5, coordinates(initial_foot_mid_rot*leg_pos[1], initial_foot_mid_rot), coordinates(initial_foot_mid_rot*leg_pos[0], initial_foot_mid_rot));
gg->go_pos_param_2_footstep_nodes_list(100*1e-3, 0, 0, boost::assign::list_of(coordinates(initial_foot_mid_rot*leg_pos[1], initial_foot_mid_rot)), start_ref_coords, boost::assign::list_of(LLEG));

if (gg->get_footstep_front_leg_names() == boost::assign::list_of("rleg")) {
std::vector<std::string> tmp_string_vector = boost::assign::list_of("rleg");
if (gg->get_footstep_front_leg_names() == tmp_string_vector) {
step_node initial_support_leg_step = step_node(LLEG, coordinates(hrp::Vector3(initial_foot_mid_rot * leg_pos[1]), initial_foot_mid_rot), 0, 0, 0, 0);
step_node initial_swing_leg_dst_step = step_node(RLEG, coordinates(hrp::Vector3(initial_foot_mid_rot * leg_pos[0]), initial_foot_mid_rot), 0, 0, 0, 0);
gen_and_plot_walk_pattern(initial_support_leg_step, initial_swing_leg_dst_step);
Expand Down

0 comments on commit a3a260e

Please sign in to comment.