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

Update 10 : support multi legs gait-generator from foot steps list #759

Merged
merged 7 commits into from
Aug 15, 2015
Merged

Update 10 : support multi legs gait-generator from foot steps list #759

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
7cc36e0
[GaitGenerator.*] move get_support_leg_types_from_footstep_nodes to g…
eisoku9618 Aug 15, 2015
554382e
[GaitGenerator.*] rename get_support_leg_types_from_footstep_nodes to…
eisoku9618 Aug 15, 2015
fcb255c
[AutoBalancer.cpp, GaitGenerator.h] move leg_type_map to gait_generator
eisoku9618 Aug 15, 2015
aff8ca9
[GaitGenerator.*] use leg_type_map in order to convert between leg_ty…
eisoku9618 Aug 15, 2015
dd12b6a
[AutoBalancer.cpp] use leg_names instead of "rleg" or "lleg"
eisoku9618 Aug 15, 2015
e824104
[AutoBalancer.cpp, GaitGenerator.*] add multi_mid_coords function to …
eisoku9618 Aug 15, 2015
2c81daa
[GaitGenerator.cpp] fix bug: keep align the order of names and coordi…
eisoku9618 Aug 15, 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
32 changes: 18 additions & 14 deletions rtc/AutoBalancer/AutoBalancer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -406,7 +406,7 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p);
} else {
for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
if (it->first == "rleg" || it->first == "lleg") {
if (std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end() ) {
it->second.current_p0 = it->second.target_link->p;
it->second.current_r0 = it->second.target_link->R;
}
Expand Down Expand Up @@ -554,9 +554,9 @@ void AutoBalancer::getTargetParameters()
gg->set_default_zmp_offsets(default_zmp_offsets);
gg_solved = gg->proc_one_tick();
{
// for support leg
std::map<leg_type, std::string> leg_type_map = boost::assign::map_list_of(RLEG, "rleg")(LLEG, "lleg")(RARM, "rarm")(LARM, "larm");
std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
coordinates tmpc;
// for support leg
for (std::vector<step_node>::const_iterator it = gg->get_support_leg_steps().begin(); it != gg->get_support_leg_steps().end(); it++) {
coordinates sp_coords = it->worldcoords;
coordinates(ikp[leg_type_map[it->l_r]].localPos,
Expand Down Expand Up @@ -668,7 +668,7 @@ void AutoBalancer::getTargetParameters()
target_root_p = m_robot->rootLink()->p;
target_root_R = m_robot->rootLink()->R;
for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
if ( control_mode == MODE_IDLE || it->first.find("leg") == std::string::npos ) {
if ( control_mode == MODE_IDLE || std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end() ) {
it->second.target_p0 = it->second.target_link->p;
it->second.target_r0 = it->second.target_link->R;
}
Expand All @@ -683,7 +683,7 @@ void AutoBalancer::getTargetParameters()
// for foot_mid_pos
tmp_foot_mid_pos += tmpikp.target_link->p + tmpikp.target_link->R * tmpikp.localPos + tmpikp.target_link->R * tmpikp.localR * default_zmp_offsets[i];
}
tmp_foot_mid_pos *= 0.5;
tmp_foot_mid_pos *= (1.0 / leg_names.size());

//
{
Expand Down Expand Up @@ -719,7 +719,7 @@ void AutoBalancer::getTargetParameters()
current_root_p = target_root_p;
current_root_R = target_root_R;
for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
if ( it->first.find("leg") != std::string::npos ) {
if ( std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end() ) {
it->second.target_p0 = it->second.target_link->p;
it->second.target_r0 = it->second.target_link->R;
}
Expand All @@ -741,16 +741,16 @@ hrp::Matrix33 AutoBalancer::OrientRotationMatrix (const hrp::Matrix33& rot, cons
void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matrix33& fix_rot)
{
// get current foot mid pos + rot
std::vector<hrp::Vector3> foot_pos;
std::vector<hrp::Matrix33> foot_rot;
std::vector<coordinates> foot_coords;
for (size_t i = 0; i < leg_names.size(); i++) {
ABCIKparam& tmpikp = ikp[leg_names[i]];
foot_pos.push_back(tmpikp.target_link->p + tmpikp.target_link->R * tmpikp.localPos);
foot_rot.push_back(tmpikp.target_link->R * tmpikp.localR);
foot_coords.push_back(coordinates((tmpikp.target_link->p + tmpikp.target_link->R * tmpikp.localPos),
(tmpikp.target_link->R * tmpikp.localR)));
}
hrp::Vector3 current_foot_mid_pos ((foot_pos[0]+foot_pos[1])/2.0);
hrp::Matrix33 current_foot_mid_rot;
mid_rot(current_foot_mid_rot, 0.5, foot_rot[0], foot_rot[1]);
coordinates current_foot_mid_coords;
multi_mid_coords(current_foot_mid_coords, foot_coords);
hrp::Vector3 current_foot_mid_pos = current_foot_mid_coords.pos;
hrp::Matrix33 current_foot_mid_rot = current_foot_mid_coords.rot;
// fix root pos + rot to fix "coords" = "current_foot_mid_xx"
hrp::Matrix33 tmpR (fix_rot * current_foot_mid_rot.transpose());
m_robot->rootLink()->p = fix_pos + tmpR * (m_robot->rootLink()->p - current_foot_mid_pos);
Expand Down Expand Up @@ -917,7 +917,11 @@ void AutoBalancer::startWalking ()

void AutoBalancer::stopWalking ()
{
mid_coords(fix_leg_coords, 0.5, ikp["rleg"].target_end_coords, ikp["lleg"].target_end_coords);
std::vector<coordinates> tmp_end_coords_list;
for (std::vector<string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) {
tmp_end_coords_list.push_back(ikp[*it].target_end_coords);
}
multi_mid_coords(fix_leg_coords, tmp_end_coords_list);
fixLegToCoords(fix_leg_coords.pos, fix_leg_coords.rot);
gg->clear_footstep_nodes_list();
if (return_control_mode == MODE_IDLE) stopABCparam();
Expand Down
80 changes: 46 additions & 34 deletions rtc/AutoBalancer/GaitGenerator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,32 +34,21 @@ namespace rats
u(2), v(2), 1;
ret = dvm * cycloid_point + start + uz;
};

std::vector<leg_type> get_support_leg_types_from_footstep_nodes(const std::vector<step_node>& fns, std::vector<std::string> _all_limbs) {
std::vector<std::string> fns_names, cntr_legs_names;
std::vector<leg_type> ret;
for (size_t i = 0; i < fns.size(); i++) {
switch (fns.at(i).l_r) {
case RLEG: fns_names.push_back("rleg"); break;
case LLEG: fns_names.push_back("lleg"); break;
case RARM: fns_names.push_back("rarm"); break;
case LARM: fns_names.push_back("larm"); break;
default: std::cerr << "invalid input" << std::endl;
}
}
std::sort(_all_limbs.begin(), _all_limbs.end());
std::sort(fns_names.begin(), fns_names.end());
std::set_difference(_all_limbs.begin(), _all_limbs.end(), /* all candidates for legs */
fns_names.begin(), fns_names.end(), /* support legs */
std::back_inserter(cntr_legs_names)); /* swing legs */
for (size_t i = 0; i < cntr_legs_names.size(); i++) {
if (cntr_legs_names.at(i) == "rleg") ret.push_back(RLEG);
else if (cntr_legs_names.at(i) == "lleg") ret.push_back(LLEG);
else if (cntr_legs_names.at(i) == "rarm") ret.push_back(RARM);
else if (cntr_legs_names.at(i) == "larm") ret.push_back(LARM);
else std::cerr << "invalid input" << std::endl;
}
return ret;
void multi_mid_coords (coordinates& ret, const std::vector<coordinates>& cs)
{
if (cs.size() == 1) {
ret = cs.front();
} else {
std::vector<coordinates> tmp_mid_coords;
double ratio = (1.0 - 1.0 / cs.size());
for (size_t i = 1; i < cs.size(); i++) {
coordinates tmp;
mid_coords(tmp, ratio, cs.front(), cs.at(i));
tmp_mid_coords.push_back(tmp);
}
multi_mid_coords(ret, tmp_mid_coords);
}
return;
};

/* member function implementation for refzmp_generator */
Expand Down Expand Up @@ -456,9 +445,14 @@ namespace rats
/* clear all gait_parameter */
size_t one_step_len = footstep_nodes_list.front().front().step_time / dt;
finalize_count = 0;
for (size_t i = 0; i < footstep_nodes_list.front().size(); i++) {
/* initial_swing_leg_dst_steps has dummy step_height, step_time, toe_angle and heel_angle. */
footstep_nodes_list.front().at(i).worldcoords = initial_swing_leg_dst_steps.at(i).worldcoords;
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++) {
if (it_fns->l_r == it_init->l_r) {
/* initial_swing_leg_dst_steps has dummy step_height, step_time, toe_angle and heel_angle. */
it_fns->worldcoords = it_init->worldcoords;
break;
}
}
}
rg.reset(one_step_len);
rg.push_refzmp_from_footstep_nodes_for_dual(footstep_nodes_list.front(), initial_support_leg_steps, initial_swing_leg_dst_steps);
Expand Down Expand Up @@ -590,19 +584,19 @@ namespace rats
dr = start_ref_coords.rot.transpose() * dr;
}
for (size_t i = 0; i < optional_go_pos_finalize_footstep_num; i++) {
append_go_pos_step_nodes(start_ref_coords, get_support_leg_types_from_footstep_nodes(footstep_nodes_list.back(), all_limbs));
append_go_pos_step_nodes(start_ref_coords, calc_counter_leg_types_from_footstep_nodes(footstep_nodes_list.back(), all_limbs));
}

/* finalize */
// Align last foot
append_go_pos_step_nodes(start_ref_coords, get_support_leg_types_from_footstep_nodes(footstep_nodes_list.back(), all_limbs));
append_go_pos_step_nodes(start_ref_coords, calc_counter_leg_types_from_footstep_nodes(footstep_nodes_list.back(), all_limbs));
// Check align
coordinates final_step_coords1 = footstep_nodes_list[footstep_nodes_list.size()-2].front().worldcoords; // Final coords in footstep_node_list
coordinates final_step_coords2 = start_ref_coords; // Final coords calculated from start_ref_coords + translate pos
final_step_coords2.pos += final_step_coords2.rot * hrp::Vector3(footstep_param.leg_default_translate_pos[footstep_nodes_list[footstep_nodes_list.size()-2].front().l_r]);
final_step_coords1.difference(dp, dr, final_step_coords2);
if ( !(eps_eq(dp.norm(), 0.0, 1e-3*0.1) && eps_eq(dr.norm(), 0.0, deg2rad(0.5))) ) { // If final_step_coords1 != final_step_coords2, add steps to match final_step_coords1 and final_step_coords2
append_go_pos_step_nodes(start_ref_coords, get_support_leg_types_from_footstep_nodes(footstep_nodes_list.back(), all_limbs));
append_go_pos_step_nodes(start_ref_coords, calc_counter_leg_types_from_footstep_nodes(footstep_nodes_list.back(), all_limbs));
}
// For Last double support period
append_finalize_footstep();
Expand Down Expand Up @@ -680,7 +674,7 @@ namespace rats

ref_coords.pos += ref_coords.rot * trans;
ref_coords.rotate(dth, hrp::Vector3(0,0,1));
append_go_pos_step_nodes(ref_coords, get_support_leg_types_from_footstep_nodes(footstep_nodes_list.back(), all_limbs));
append_go_pos_step_nodes(ref_coords, calc_counter_leg_types_from_footstep_nodes(footstep_nodes_list.back(), all_limbs));
};

void gait_generator::calc_next_coords_velocity_mode (std::vector< std::vector<coordinates> >& ret_list, const size_t idx)
Expand All @@ -692,7 +686,7 @@ namespace rats

std::vector<leg_type> cur_sup_legs, next_sup_legs;
for (size_t i = 0; i < footstep_nodes_list[idx-1].size(); i++) cur_sup_legs.push_back(footstep_nodes_list[idx-1].at(i).l_r);
next_sup_legs = get_support_leg_types_from_footstep_nodes(footstep_nodes_list[idx-1], all_limbs);
next_sup_legs = calc_counter_leg_types_from_footstep_nodes(footstep_nodes_list[idx-1], all_limbs);

for (size_t i = 0; i < 3; i++) {
std::vector<coordinates> ret;
Expand Down Expand Up @@ -757,5 +751,23 @@ namespace rats
rg.update_refzmp(footstep_nodes_list);
}
};

const std::vector<leg_type> gait_generator::calc_counter_leg_types_from_footstep_nodes(const std::vector<step_node>& fns, std::vector<std::string> _all_limbs) const {
std::vector<std::string> fns_names, cntr_legs_names;
for (std::vector<step_node>::const_iterator it = fns.begin(); it != fns.end(); it++) {
fns_names.push_back(leg_type_map.find(it->l_r)->second);
}
std::sort(_all_limbs.begin(), _all_limbs.end());
std::sort(fns_names.begin(), fns_names.end());
std::set_difference(_all_limbs.begin(), _all_limbs.end(), /* all candidates for legs */
fns_names.begin(), fns_names.end(), /* support legs */
std::back_inserter(cntr_legs_names)); /* swing legs */
std::vector<leg_type> ret;
for (std::vector<std::string>::const_iterator it = cntr_legs_names.begin(); it != cntr_legs_names.end(); it++) {
std::map<leg_type, std::string>::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == *it));
ret.push_back(dst->first);
}
return ret;
};
}

34 changes: 22 additions & 12 deletions rtc/AutoBalancer/GaitGenerator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@ namespace rats
const double ratio, const hrp::Vector3& start,
const hrp::Vector3& goal, const double height,
const double default_top_ratio = 0.5);
void multi_mid_coords (coordinates& mid_coords, const std::vector<coordinates>& cs);

enum orbit_type {SHUFFLING, CYCLOID, RECTANGLE, STAIR, CYCLOIDDELAY, CYCLOIDDELAYKICK};
enum leg_type {RLEG, LLEG, RARM, LARM, BOTH, ALL};
Expand Down Expand Up @@ -55,7 +56,6 @@ namespace rats
return os;
};
};
std::vector<leg_type> get_support_leg_types_from_footstep_nodes(const std::vector<step_node>& fns, std::vector<std::string> _all_limbs);

/* footstep parameter */
struct footstep_parameter
Expand Down Expand Up @@ -686,9 +686,21 @@ namespace rats
double get_default_step_height () const { return default_step_height;};
void get_swing_support_mid_coords(coordinates& ret) const
{
coordinates tmp;
mid_coords(tmp, foot_midcoords_ratio, swing_leg_src_steps.front().worldcoords, swing_leg_dst_steps.front().worldcoords);
mid_coords(ret, 0.5, tmp, support_leg_steps.front().worldcoords);
std::vector<coordinates> swg_coords, sup_coords;
for (std::vector<step_node>::const_iterator it_src = swing_leg_src_steps.begin(), it_dst = swing_leg_dst_steps.begin();
it_src != swing_leg_src_steps.end() && it_dst != swing_leg_dst_steps.end();
it_src++, it_dst++) {
coordinates tmp;
mid_coords(tmp, foot_midcoords_ratio, it_src->worldcoords, it_dst->worldcoords);
swg_coords.push_back(tmp);
}
for (std::vector<step_node>::const_iterator it = support_leg_steps.begin(); it != support_leg_steps.end(); it++) {
sup_coords.push_back(it->worldcoords);
}
coordinates tmp_swg_mid, tmp_sup_mid;
multi_mid_coords(tmp_swg_mid, swg_coords);
multi_mid_coords(tmp_sup_mid, sup_coords);
mid_coords(ret, 0.5, tmp_swg_mid, tmp_sup_mid);
};
std::vector<leg_type> get_current_support_states () const
{
Expand Down Expand Up @@ -759,6 +771,7 @@ namespace rats
velocity_mode_flag velocity_mode_flg;
emergency_flag emergency_flg;
bool use_inside_step_limitation;
std::map<leg_type, std::string> leg_type_map;

/* preview controller parameters */
//preview_dynamics_filter<preview_control>* preview_controller_ptr;
Expand Down Expand Up @@ -801,6 +814,7 @@ namespace rats
preview_controller_ptr(NULL) {
swing_foot_zmp_offsets = boost::assign::list_of<hrp::Vector3>(hrp::Vector3::Zero());
prev_que_sfzos = boost::assign::list_of<hrp::Vector3>(hrp::Vector3::Zero());
leg_type_map = boost::assign::map_list_of<leg_type, std::string>(RLEG, "rleg")(LLEG, "lleg")(RARM, "rarm")(LARM, "larm");
};
~gait_generator () {
if ( preview_controller_ptr != NULL ) {
Expand Down Expand Up @@ -961,14 +975,8 @@ namespace rats
};
std::vector<std::string> convert_leg_types_to_names (const std::vector<leg_type>& lts) const {
std::vector<std::string> ret;
for (size_t i = 0; i < lts.size(); i++) {
switch(lts.at(i)) {
case RLEG : ret.push_back("rleg"); break;
case LLEG : ret.push_back("lleg"); break;
case RARM : ret.push_back("rarm"); break;
case LARM : ret.push_back("rarm"); break;
default : ret.push_back("lleg"); break;
}
for (std::vector<leg_type>::const_iterator it = lts.begin(); it != lts.end(); it++) {
ret.push_back(leg_type_map.find(*it)->second);
}
return ret;
};
Expand Down Expand Up @@ -1050,6 +1058,8 @@ namespace rats
int get_NUM_TH_PHASES () { return thp.get_NUM_TH_PHASES(); };
bool get_use_toe_joint () { return lcg.get_use_toe_joint(); };
void get_leg_default_translate_pos (std::vector<hrp::Vector3>& off) { off = footstep_param.leg_default_translate_pos; };
const std::vector<leg_type> calc_counter_leg_types_from_footstep_nodes (const std::vector<step_node>& fns, std::vector<std::string> _all_limbs) const;
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; };
void print_param (const std::string& print_str = "")
Expand Down