Skip to content

Commit

Permalink
[AutoBalancer.cpp, GaitGenerator.*] add zmp weight map which is used …
Browse files Browse the repository at this point in the history 
…in target zmp calculation
  • Loading branch information
@eisoku9618
eisoku9618 committed Aug 26, 2015
1 parent 0a17e26 commit 0c338c6
Show file tree
Hide file tree
Showing 3 changed files with 34 additions and 16 deletions.
24 changes: 15 additions & 9 deletions rtc/AutoBalancer/AutoBalancer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -675,16 +675,22 @@ void AutoBalancer::getTargetParameters()
}

hrp::Vector3 tmp_foot_mid_pos(hrp::Vector3::Zero());
for (size_t i = 0; i < leg_names.size(); i++) {
ABCIKparam& tmpikp = ikp[leg_names[i]];
// get target_end_coords
tmpikp.target_end_coords.pos = tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos;
tmpikp.target_end_coords.rot = tmpikp.target_r0 * tmpikp.localR;
// 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];
{
std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
std::map<leg_type, double> zmp_weight_map = gg->get_zmp_weight_map();
double sum_of_weight = 0.0;
for (size_t i = 0; i < leg_names.size(); i++) {
ABCIKparam& tmpikp = ikp[leg_names[i]];
// get target_end_coords
tmpikp.target_end_coords.pos = tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos;
tmpikp.target_end_coords.rot = tmpikp.target_r0 * tmpikp.localR;
// for foot_mid_pos
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 == leg_names[i]));
tmp_foot_mid_pos += (tmpikp.target_link->p + tmpikp.target_link->R * tmpikp.localPos + tmpikp.target_link->R * tmpikp.localR * default_zmp_offsets[i]) * zmp_weight_map[dst->first];
sum_of_weight += zmp_weight_map[dst->first];
}
tmp_foot_mid_pos *= (1.0 / sum_of_weight);
}
tmp_foot_mid_pos *= (1.0 / leg_names.size());

//
{
if ( gg_is_walking && gg->get_lcg_count() == static_cast<size_t>(gg->get_default_step_time()/(2*m_dt))-1) {
Expand Down
20 changes: 13 additions & 7 deletions rtc/AutoBalancer/GaitGenerator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,14 +61,18 @@ namespace rats
hrp::Vector3 ret_zmp;
hrp::Vector3 tmp_zero = hrp::Vector3::Zero();
std::vector<hrp::Vector3> foot_x_axises;
for (std::vector<step_node>::const_iterator it = _support_leg_steps.begin(); it != _support_leg_steps.end(); it++)
dzl.push_back(it->worldcoords.rot * default_zmp_offsets[it->l_r] + it->worldcoords.pos);
double sum_of_weight = 0.0;
for (std::vector<step_node>::const_iterator it = _support_leg_steps.begin(); it != _support_leg_steps.end(); it++) {
dzl.push_back((it->worldcoords.rot * default_zmp_offsets[it->l_r] + it->worldcoords.pos) * zmp_weight_map[it->l_r]);
sum_of_weight += zmp_weight_map[it->l_r];
}
for (std::vector<step_node>::const_iterator it = _swing_leg_steps.begin(); it != _swing_leg_steps.end(); it++) {
dzl.push_back(it->worldcoords.rot * default_zmp_offsets[it->l_r] + it->worldcoords.pos);
dzl.push_back((it->worldcoords.rot * default_zmp_offsets[it->l_r] + it->worldcoords.pos) * zmp_weight_map[it->l_r]);
sum_of_weight += zmp_weight_map[it->l_r];
foot_x_axises.push_back( hrp::Vector3(it->worldcoords.rot * hrp::Vector3::UnitX()) );
}
foot_x_axises_list.push_back(foot_x_axises);
rzmp = std::accumulate(dzl.begin(), dzl.end(), tmp_zero) / dzl.size();
rzmp = std::accumulate(dzl.begin(), dzl.end(), tmp_zero) / sum_of_weight;
refzmp_cur_list.push_back( rzmp );
std::vector<leg_type> swing_leg_types;
for (size_t i = 0; i < fns.size(); i++) {
Expand All @@ -86,12 +90,14 @@ namespace rats
hrp::Vector3 rzmp, tmp_zero=hrp::Vector3::Zero();
std::vector<hrp::Vector3> dzl;
std::vector<hrp::Vector3> foot_x_axises;
double sum_of_weight = 0.0;

for (std::vector<step_node>::const_iterator it = _support_leg_steps.begin(); it != _support_leg_steps.end(); it++) {
dzl.push_back(it->worldcoords.rot * default_zmp_offsets[it->l_r] + it->worldcoords.pos);
foot_x_axises.push_back( hrp::Vector3(it->worldcoords.rot * hrp::Vector3::UnitX()) );
dzl.push_back((it->worldcoords.rot * default_zmp_offsets[it->l_r] + it->worldcoords.pos) * zmp_weight_map[it->l_r]);
sum_of_weight += zmp_weight_map[it->l_r];
foot_x_axises.push_back( hrp::Vector3(it->worldcoords.rot * hrp::Vector3::UnitX()) );
}
rzmp = std::accumulate(dzl.begin(), dzl.end(), tmp_zero) / dzl.size();
rzmp = std::accumulate(dzl.begin(), dzl.end(), tmp_zero) / sum_of_weight;
refzmp_cur_list.push_back( rzmp );
foot_x_axises_list.push_back(foot_x_axises);
std::vector<leg_type> swing_leg_types;
Expand Down
6 changes: 6 additions & 0 deletions rtc/AutoBalancer/GaitGenerator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -204,6 +204,7 @@ namespace rats
public:
#endif
std::vector<hrp::Vector3> refzmp_cur_list;
std::map<leg_type, double> zmp_weight_map;
std::vector< std::vector<hrp::Vector3> > foot_x_axises_list; // Swing foot x axis list according to refzmp_cur_list
std::vector< std::vector<leg_type> > swing_leg_types_list; // Swing leg list according to refzmp_cur_list
std::vector<size_t> step_count_list; // Swing leg list according to refzmp_cur_list
Expand Down Expand Up @@ -231,6 +232,7 @@ namespace rats
default_zmp_offsets.push_back(hrp::Vector3::Zero());
default_zmp_offsets.push_back(hrp::Vector3::Zero());
default_zmp_offsets.push_back(hrp::Vector3::Zero());
zmp_weight_map = boost::assign::map_list_of<leg_type, double>(RLEG, 1.0)(LLEG, 1.0)(RARM, 0.01)(LARM, 0.01);
};
~refzmp_generator()
{
Expand Down Expand Up @@ -265,6 +267,7 @@ namespace rats
void set_toe_zmp_offset_x (const double _off) { toe_zmp_offset_x = _off; };
void set_heel_zmp_offset_x (const double _off) { heel_zmp_offset_x = _off; };
void set_use_toe_heel_transition (const double _u) { use_toe_heel_transition = _u; };
void set_zmp_weight_map (const std::map<leg_type, double> _map) { zmp_weight_map = _map; };
// getter
bool get_current_refzmp (hrp::Vector3& rzmp, std::vector<hrp::Vector3>& swing_foot_zmp_offsets, const double default_double_support_ratio, const double default_double_support_static_ratio) const
{
Expand All @@ -276,6 +279,7 @@ namespace rats
double get_toe_zmp_offset_x () const { return toe_zmp_offset_x; };
double get_heel_zmp_offset_x () const { return heel_zmp_offset_x; };
bool get_use_toe_heel_transition () const { return use_toe_heel_transition; };
const std::map<leg_type, double> get_zmp_weight_map () const { return zmp_weight_map; };
};

class delay_hoffarbib_trajectory_generator
Expand Down Expand Up @@ -887,6 +891,7 @@ namespace rats
void set_toe_zmp_offset_x (const double _off) { rg.set_toe_zmp_offset_x(_off); };
void set_heel_zmp_offset_x (const double _off) { rg.set_heel_zmp_offset_x(_off); };
void set_use_toe_heel_transition (const double _u) { rg.set_use_toe_heel_transition(_u); };
void set_zmp_weight_map (const std::map<leg_type, double> _map) { rg.set_zmp_weight_map(_map); };
void set_default_step_height(const double _tmp) { lcg.set_default_step_height(_tmp); };
void set_default_top_ratio(const double _tmp) { lcg.set_default_top_ratio(_tmp); };
void set_velocity_param (const double vel_x, const double vel_y, const double vel_theta) /* [mm/s] [mm/s] [deg/s] */
Expand Down Expand Up @@ -996,6 +1001,7 @@ namespace rats
double get_toe_zmp_offset_x () const { return rg.get_toe_zmp_offset_x(); };
double get_heel_zmp_offset_x () const { return rg.get_heel_zmp_offset_x(); };
bool get_use_toe_heel_transition () const { return rg.get_use_toe_heel_transition(); };
const std::map<leg_type, double> get_zmp_weight_map () const { return rg.get_zmp_weight_map(); };
std::vector<std::string> get_footstep_front_leg_names () const {
std::vector<leg_type> lts;
for (size_t i = 0; i < footstep_nodes_list[0].size(); i++) {
Expand Down

0 comments on commit 0c338c6

Please sign in to comment.