GaitGenerator.cpp
Go to the documentation of this file.
00001 /* -*- coding:utf-8-unix; mode:c++; -*- */
00002 
00003 #include "GaitGenerator.h"
00004 #include <numeric>
00005 
00006 namespace rats
00007 {
00008 #ifndef rad2deg
00009 #define rad2deg(rad) (rad * 180 / M_PI)
00010 #endif
00011 #ifndef deg2rad
00012 #define deg2rad(deg) (deg * M_PI / 180)
00013 #endif
00014   void cycloid_midpoint (hrp::Vector3& ret,
00015                          const double ratio, const hrp::Vector3& start,
00016                          const hrp::Vector3& goal, const double height,
00017                          const double default_top_ratio)
00018   {
00019     hrp::Vector3 u ( goal - start );
00020     hrp::Vector3 uz (0,0, ratio * u(2));
00021     u(2) = 0.0;
00022     double pth = 2 * M_PI * ratio, norm_u = u.norm();
00023     if ( !eps_eq(norm_u, 0.0,1e-3*0.01) )
00024       u =  u.normalized();
00025     /* check ratio vs 0.5 for default_top_ratio blending */
00026     hrp::Vector3 cycloid_point( ((0.5 > ratio) ? ( 2 * default_top_ratio * norm_u ) : ( 2 * (1 - default_top_ratio) * norm_u )) * ( pth - sin(pth) ) / (2 * M_PI) -
00027                            ((0.5 > ratio) ? 0.0 : (norm_u * (1 - 2 * default_top_ratio)) ), // local x
00028                            0, // local y
00029                            ( 0.5 * height * ( 1 - cos(pth) )) ); // local z
00030     hrp::Vector3 v(hrp::Vector3(0,0,1).cross(u));
00031     hrp::Matrix33 dvm;
00032     dvm << u(0), v(0), 0,
00033       u(1), v(1), 0,
00034       u(2), v(2), 1;
00035     ret = dvm * cycloid_point + start + uz;
00036   };
00037   void multi_mid_coords (coordinates& ret, const std::vector<coordinates>& cs, const double eps)
00038   {
00039       if (cs.size() == 1) {
00040           ret = cs.front();
00041       } else {
00042           std::vector<coordinates> tmp_mid_coords;
00043           double ratio = (1.0 - 1.0 / cs.size());
00044           for (size_t i = 1; i < cs.size(); i++) {
00045               coordinates tmp;
00046               mid_coords(tmp, ratio, cs.front(), cs.at(i), eps);
00047               tmp_mid_coords.push_back(tmp);
00048           }
00049           multi_mid_coords(ret, tmp_mid_coords, eps);
00050       }
00051       return;
00052   };
00053 
00054   std::string leg_type_to_leg_type_string (const leg_type l_r)
00055   {
00056       return ((l_r==LLEG)?std::string("lleg"):
00057               (l_r==RARM)?std::string("rarm"):
00058               (l_r==LARM)?std::string("larm"):
00059               std::string("rleg"));
00060   };
00061 
00062   double set_value_according_to_toe_heel_type (const toe_heel_type tht, const double toe_value, const double heel_value, const double default_value)
00063   {
00064       if (tht == TOE) {
00065           return toe_value;
00066       } else if (tht == HEEL) {
00067           return heel_value;
00068       } else {
00069           return default_value;
00070       }
00071   };
00072 
00073   /* member function implementation for refzmp_generator */
00074   void refzmp_generator::push_refzmp_from_footstep_nodes_for_dual (const std::vector<step_node>& fns,
00075                                                                    const std::vector<step_node>& _support_leg_steps,
00076                                                                    const std::vector<step_node>& _swing_leg_steps)
00077   {
00078     hrp::Vector3 rzmp;
00079     std::vector<hrp::Vector3> dzl;
00080     hrp::Vector3 ret_zmp;
00081     hrp::Vector3 tmp_zero = hrp::Vector3::Zero();
00082     std::vector<hrp::Vector3> foot_x_axises;
00083     double sum_of_weight = 0.0;
00084     for (std::vector<step_node>::const_iterator it = _support_leg_steps.begin(); it != _support_leg_steps.end(); it++) {
00085         dzl.push_back((it->worldcoords.rot * default_zmp_offsets[it->l_r] + it->worldcoords.pos) * zmp_weight_map[it->l_r]);
00086         sum_of_weight += zmp_weight_map[it->l_r];
00087     }
00088     for (std::vector<step_node>::const_iterator it = _swing_leg_steps.begin(); it != _swing_leg_steps.end(); it++) {
00089         dzl.push_back((it->worldcoords.rot * default_zmp_offsets[it->l_r] + it->worldcoords.pos) * zmp_weight_map[it->l_r]);
00090         sum_of_weight += zmp_weight_map[it->l_r];
00091         foot_x_axises.push_back( hrp::Vector3(it->worldcoords.rot * hrp::Vector3::UnitX()) );
00092     }
00093     foot_x_axises_list.push_back(foot_x_axises);
00094     rzmp = std::accumulate(dzl.begin(), dzl.end(), tmp_zero) / sum_of_weight;
00095     refzmp_cur_list.push_back( rzmp );
00096     std::vector<leg_type> swing_leg_types;
00097     for (size_t i = 0; i < fns.size(); i++) {
00098         swing_leg_types.push_back(fns.at(i).l_r);
00099     }
00100     swing_leg_types_list.push_back( swing_leg_types );
00101     step_count_list.push_back(static_cast<size_t>(fns.front().step_time/dt));
00102     toe_heel_types_list.push_back(toe_heel_types(SOLE, SOLE));
00103     //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;
00104   };
00105 
00106   void refzmp_generator::push_refzmp_from_footstep_nodes_for_single (const std::vector<step_node>& fns, const std::vector<step_node>& _support_leg_steps, const toe_heel_types& tht)
00107   {
00108     // support leg = prev fns l_r
00109     // swing leg = fns l_r
00110     hrp::Vector3 rzmp, tmp_zero=hrp::Vector3::Zero();
00111     std::vector<hrp::Vector3> dzl;
00112     std::vector<hrp::Vector3> foot_x_axises;
00113     double sum_of_weight = 0.0;
00114 
00115     for (std::vector<step_node>::const_iterator it = _support_leg_steps.begin(); it != _support_leg_steps.end(); it++) {
00116         dzl.push_back((it->worldcoords.rot * default_zmp_offsets[it->l_r] + it->worldcoords.pos) * zmp_weight_map[it->l_r]);
00117         sum_of_weight += zmp_weight_map[it->l_r];
00118         foot_x_axises.push_back( hrp::Vector3(it->worldcoords.rot * hrp::Vector3::UnitX()) );
00119     }
00120     rzmp = std::accumulate(dzl.begin(), dzl.end(), tmp_zero) / sum_of_weight;
00121     refzmp_cur_list.push_back( rzmp );
00122     foot_x_axises_list.push_back(foot_x_axises);
00123     std::vector<leg_type> swing_leg_types;
00124     for (size_t i = 0; i< fns.size(); i++) {
00125         swing_leg_types.push_back(fns.at(i).l_r);
00126     }
00127     swing_leg_types_list.push_back( swing_leg_types );
00128     step_count_list.push_back(static_cast<size_t>(fns.front().step_time/dt));
00129     toe_heel_types_list.push_back(tht);
00130     //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;
00131   };
00132 
00133   void refzmp_generator::calc_current_refzmp (hrp::Vector3& ret, std::vector<hrp::Vector3>& swing_foot_zmp_offsets, const double default_double_support_ratio_before, const double default_double_support_ratio_after, const double default_double_support_static_ratio_before, const double default_double_support_static_ratio_after)
00134   {
00135     size_t cnt = one_step_count - refzmp_count; // current counter (0 -> one_step_count)
00136     size_t double_support_count_half_before = default_double_support_ratio_before * one_step_count;
00137     size_t double_support_count_half_after = default_double_support_ratio_after * one_step_count;
00138     size_t double_support_static_count_half_before = default_double_support_static_ratio_before * one_step_count;
00139     size_t double_support_static_count_half_after = default_double_support_static_ratio_after * one_step_count;
00140     for (size_t i = 0; i < swing_leg_types_list[refzmp_index].size(); i++) {
00141         swing_foot_zmp_offsets.push_back(default_zmp_offsets[swing_leg_types_list[refzmp_index].at(i)]);
00142     }
00143     double zmp_diff = 0.0; // difference between total swing_foot_zmp_offset and default_zmp_offset
00144     //if (cnt==0) std::cerr << "z " << refzmp_index << " " << refzmp_cur_list.size() << " " << fs_index << " " << (refzmp_index == refzmp_cur_list.size()-2) << " " << is_final_double_support_set << std::endl;
00145 
00146     // Calculate swing foot zmp offset for toe heel zmp transition
00147     if (use_toe_heel_transition &&
00148         !(is_start_double_support_phase() || is_end_double_support_phase())) { // Do not use toe heel zmp transition during start and end double support period because there is no swing foot
00149         double first_zmp_offset_x, second_zmp_offset_x;
00150         if (use_toe_heel_auto_set) {
00151             first_zmp_offset_x = set_value_according_to_toe_heel_type(toe_heel_types_list[refzmp_index].src_type, toe_zmp_offset_x, heel_zmp_offset_x, swing_foot_zmp_offsets.front()(0));
00152             second_zmp_offset_x = set_value_according_to_toe_heel_type(toe_heel_types_list[refzmp_index].dst_type, toe_zmp_offset_x, heel_zmp_offset_x, swing_foot_zmp_offsets.front()(0));
00153         } else {
00154             first_zmp_offset_x = toe_zmp_offset_x;
00155             second_zmp_offset_x = heel_zmp_offset_x;
00156         }
00157         if (thp.is_between_phases(cnt, SOLE0)) {
00158             double ratio = thp.calc_phase_ratio(cnt+1, SOLE0);
00159             swing_foot_zmp_offsets.front()(0) = (1-ratio)*swing_foot_zmp_offsets.front()(0) + ratio*first_zmp_offset_x;
00160         } else if (thp.is_between_phases(cnt, HEEL2SOLE, SOLE2)) {
00161             double ratio = thp.calc_phase_ratio(cnt, HEEL2SOLE, SOLE2);
00162             swing_foot_zmp_offsets.front()(0) = ratio*swing_foot_zmp_offsets.front()(0) + (1-ratio)*second_zmp_offset_x;
00163         } else if (thp.is_between_phases(cnt, SOLE0, SOLE2TOE)) {
00164             swing_foot_zmp_offsets.front()(0) = first_zmp_offset_x;
00165         } else if (thp.is_between_phases(cnt, SOLE2HEEL, HEEL2SOLE)) {
00166             swing_foot_zmp_offsets.front()(0) = second_zmp_offset_x;
00167         } else if (thp.is_between_phases(cnt, SOLE2TOE, SOLE2HEEL)) {
00168             double ratio = thp.calc_phase_ratio(cnt, SOLE2TOE, SOLE2HEEL);
00169             swing_foot_zmp_offsets.front()(0) = ratio * second_zmp_offset_x + (1-ratio) * first_zmp_offset_x;
00170         }
00171         zmp_diff = swing_foot_zmp_offsets.front()(0)-default_zmp_offsets[swing_leg_types_list[refzmp_index].front()](0);
00172         if ((is_second_phase() && ( cnt < double_support_count_half_before )) ||
00173             (is_second_last_phase() && ( cnt > one_step_count - double_support_count_half_after ))) {
00174             // "* 0.5" is for double supprot period
00175             zmp_diff *= 0.5;
00176         }
00177     }
00178 
00179     // Calculate total reference ZMP
00180     if (is_start_double_support_phase() || is_end_double_support_phase()) {
00181       ret = refzmp_cur_list[refzmp_index];
00182     } else if ( cnt < double_support_static_count_half_before ) { // Start double support static period
00183       hrp::Vector3 current_support_zmp = refzmp_cur_list[refzmp_index];
00184       hrp::Vector3 prev_support_zmp = refzmp_cur_list[refzmp_index-1] + zmp_diff * foot_x_axises_list[refzmp_index-1].front();
00185       double ratio = (is_second_phase()?1.0:0.5);
00186       ret = (1 - ratio) * current_support_zmp + ratio * prev_support_zmp;
00187     } else if ( cnt > one_step_count - double_support_static_count_half_after ) { // End double support static period
00188       hrp::Vector3 current_support_zmp = refzmp_cur_list[refzmp_index+1] + zmp_diff * foot_x_axises_list[refzmp_index+1].front();
00189       hrp::Vector3 prev_support_zmp = refzmp_cur_list[refzmp_index];
00190       double ratio = (is_second_last_phase()?1.0:0.5);
00191       ret = (1 - ratio) * prev_support_zmp + ratio * current_support_zmp;
00192     } else if ( cnt < double_support_count_half_before ) { // Start double support period
00193       hrp::Vector3 current_support_zmp = refzmp_cur_list[refzmp_index];
00194       hrp::Vector3 prev_support_zmp = refzmp_cur_list[refzmp_index-1] + zmp_diff * foot_x_axises_list[refzmp_index-1].front();
00195       double ratio = ((is_second_phase()?1.0:0.5) / (double_support_count_half_before-double_support_static_count_half_before)) * (double_support_count_half_before-cnt);
00196       ret = (1 - ratio) * current_support_zmp + ratio * prev_support_zmp;
00197     } else if ( cnt > one_step_count - double_support_count_half_after ) { // End double support period
00198       hrp::Vector3 current_support_zmp = refzmp_cur_list[refzmp_index+1] + zmp_diff * foot_x_axises_list[refzmp_index+1].front();
00199       hrp::Vector3 prev_support_zmp = refzmp_cur_list[refzmp_index];
00200       double ratio = ((is_second_last_phase()?1.0:0.5) / (double_support_count_half_after-double_support_static_count_half_after)) * (cnt - 1 - (one_step_count - double_support_count_half_after));
00201       ret = (1 - ratio) * prev_support_zmp + ratio * current_support_zmp;
00202     } else {
00203       ret = refzmp_cur_list[refzmp_index];
00204     }
00205   };
00206 
00207   void refzmp_generator::update_refzmp ()
00208   {
00209     if ( 1 <= refzmp_count ) {
00210       refzmp_count--;
00211     } else {
00212       refzmp_index++;
00213       // Check length of step_count_list and refzmp_index
00214       //   The case if !(refzmp_index <= step_count_list.size()-1) is finalizing of gait_generator.
00215       //   If finalizing, this can be neglected.
00216       if (refzmp_index <= step_count_list.size()-1) {
00217           refzmp_count = one_step_count = step_count_list[refzmp_index];
00218           thp.set_one_step_count(one_step_count);
00219       }
00220       //std::cerr << "fs " << fs_index << "/" << fnl.size() << " rf " << refzmp_index << "/" << refzmp_cur_list.size() << " flg " << std::endl;
00221     }
00222   };
00223 
00224   void leg_coords_generator::calc_current_swing_foot_rot (std::map<leg_type, hrp::Vector3>& tmp_swing_foot_rot, const double _default_double_support_ratio_before, const double _default_double_support_ratio_after)
00225   {
00226     // interpolation
00227     int support_len_before = one_step_count * _default_double_support_ratio_before;
00228     int support_len_after = one_step_count * _default_double_support_ratio_after;
00229     int current_swing_count = (one_step_count - lcg_count); // 0->one_step_count
00230     // swing foot rot interpolator interpolates difference from src to dst.
00231     if (current_swing_count == support_len_before) {
00232         for (std::vector<step_node>::iterator it = swing_leg_src_steps.begin(); it != swing_leg_src_steps.end(); it++) {
00233             swing_foot_rot_interpolator[it->l_r]->clear();
00234             double tmp[3] = {};
00235             swing_foot_rot_interpolator[it->l_r]->set(tmp);
00236         }
00237         int swing_len = one_step_count - support_len_before - support_len_after;
00238         for (size_t ii = 0; ii < swing_leg_dst_steps.size(); ii++) {
00239             leg_type lt = swing_leg_dst_steps[ii].l_r;
00240             swing_foot_rot_interpolator[lt]->setGoal(hrp::rpyFromRot(swing_leg_src_steps[ii].worldcoords.rot.transpose() * swing_leg_dst_steps[ii].worldcoords.rot).data(),
00241                                                      dt * swing_len);
00242             swing_foot_rot_interpolator[lt]->sync();
00243         }
00244     } else if ( (current_swing_count > support_len_before) && (current_swing_count < (one_step_count-support_len_after) ) ) {
00245         int tmp_len = (lcg_count - support_len_after);
00246         for (size_t ii = 0; ii < swing_leg_dst_steps.size(); ii++) {
00247             leg_type lt = swing_leg_dst_steps[ii].l_r;
00248             swing_foot_rot_interpolator[lt]->setGoal(hrp::rpyFromRot(swing_leg_src_steps[ii].worldcoords.rot.transpose() * swing_leg_dst_steps[ii].worldcoords.rot).data(),
00249                                                      dt * tmp_len);
00250             swing_foot_rot_interpolator[lt]->sync();
00251         }
00252     }
00253     for (size_t ii = 0; ii < swing_leg_dst_steps.size(); ii++) {
00254         hrp::Vector3 tmpv;
00255         if ( !swing_foot_rot_interpolator[swing_leg_dst_steps[ii].l_r]->isEmpty() ) {
00256             swing_foot_rot_interpolator[swing_leg_dst_steps[ii].l_r]->get(tmpv.data(), true);
00257         } else {
00258             if ( (current_swing_count < support_len_before) ) {
00259                 tmpv  = hrp::Vector3::Zero();
00260             } else if (current_swing_count >= (one_step_count-support_len_after)) {
00261                 tmpv  = hrp::rpyFromRot(swing_leg_src_steps[ii].worldcoords.rot.transpose() * swing_leg_dst_steps[ii].worldcoords.rot);
00262             }
00263         }
00264         tmp_swing_foot_rot.insert(std::pair<leg_type, hrp::Vector3>(swing_leg_dst_steps[ii].l_r, tmpv));
00265     }
00266   };
00267 
00268   /* member function implementation for leg_coords_generator */
00269   void leg_coords_generator::calc_current_swing_leg_steps (std::vector<step_node>& rets, const double step_height, const double _current_toe_angle, const double _current_heel_angle, const double _default_double_support_ratio_before, const double _default_double_support_ratio_after)
00270   {
00271     /* match the src step order and the dst step order */
00272     std::sort(swing_leg_src_steps.begin(), swing_leg_src_steps.end(),
00273               ((&boost::lambda::_1->* &step_node::l_r) < (&boost::lambda::_2->* &step_node::l_r)));
00274     std::sort(swing_leg_dst_steps.begin(), swing_leg_dst_steps.end(),
00275               ((&boost::lambda::_1->* &step_node::l_r) < (&boost::lambda::_2->* &step_node::l_r)));
00276     std::map<leg_type, hrp::Vector3> tmp_swing_foot_rot;
00277     calc_current_swing_foot_rot(tmp_swing_foot_rot, _default_double_support_ratio_before, _default_double_support_ratio_after);
00278     size_t swing_trajectory_generator_idx = 0;
00279     for (std::vector<step_node>::iterator it1 = swing_leg_src_steps.begin(), it2 = swing_leg_dst_steps.begin();
00280          it1 != swing_leg_src_steps.end() && it2 != swing_leg_dst_steps.end();
00281          it1++, it2++) {
00282       coordinates ret;
00283       ret.rot = it1->worldcoords.rot * hrp::rotFromRpy(tmp_swing_foot_rot[it2->l_r]);
00284       switch (default_orbit_type) {
00285       case SHUFFLING:
00286         ret.pos = swing_ratio*it1->worldcoords.pos + (1-swing_ratio)*it2->worldcoords.pos;
00287         break;
00288       case CYCLOID:
00289         cycloid_midcoords(ret, it1->worldcoords, it2->worldcoords, step_height);
00290         break;
00291       case RECTANGLE:
00292         rectangle_midcoords(ret, it1->worldcoords, it2->worldcoords, step_height, swing_trajectory_generator_idx);
00293         break;
00294       case STAIR:
00295         stair_midcoords(ret, it1->worldcoords, it2->worldcoords, step_height);
00296         break;
00297       case CYCLOIDDELAY:
00298         cycloid_delay_midcoords(ret, it1->worldcoords, it2->worldcoords, step_height, swing_trajectory_generator_idx);
00299         break;
00300       case CYCLOIDDELAYKICK:
00301         cycloid_delay_kick_midcoords(ret, it1->worldcoords, it2->worldcoords, step_height);
00302         break;
00303       case CROSS:
00304         cross_delay_midcoords(ret, it1->worldcoords, it2->worldcoords, step_height, it1->l_r);
00305         break;
00306       default: break;
00307       }
00308       swing_trajectory_generator_idx++;
00309       if (std::fabs(step_height) > 1e-3*10) {
00310           if (swing_leg_src_steps.size() == 1) /* only biped or crawl because there is only one toe_heel_interpolator */
00311               modif_foot_coords_for_toe_heel_phase(ret, _current_toe_angle, _current_heel_angle);
00312       }
00313       rets.push_back(step_node(it1->l_r, ret, 0, 0, 0, 0));
00314     }
00315   };
00316 
00317   void leg_coords_generator::calc_ratio_from_double_support_ratio (const double default_double_support_ratio_before, const double default_double_support_ratio_after)
00318   {
00319     int support_len_before = one_step_count * default_double_support_ratio_before;
00320     int support_len_after = one_step_count * default_double_support_ratio_after;
00321     // int support_len = 2*static_cast<int>(one_step_count * default_double_support_ratio * 0.5);
00322     int swing_len = one_step_count - support_len_before - support_len_after;
00323     int current_swing_len = lcg_count - support_len_before;
00324     double tmp_current_swing_time;
00325     int current_swing_count = (one_step_count - lcg_count); // 0->one_step_count
00326     if ( current_swing_count < support_len_before ) { // First double support period
00327       swing_ratio = 0.0;
00328       tmp_current_swing_time = current_swing_len * dt - swing_len * dt;
00329       is_swing_phase = false;
00330     } else if ( current_swing_count >= support_len_before+swing_len ) { // Last double support period
00331       swing_ratio = 1.0;
00332       tmp_current_swing_time = current_swing_len * dt + (support_len_before + support_len_after + next_one_step_count) * dt;
00333       is_swing_phase = false;
00334     } else {
00335       tmp_current_swing_time = current_swing_len * dt;
00336       swing_ratio = static_cast<double>(current_swing_count-support_len_before)/swing_len;
00337       //std::cerr << "gp " << swing_ratio << " " << swing_rot_ratio << std::endl;
00338       if (current_step_height > 0.0) is_swing_phase = true;
00339       else is_swing_phase = false;
00340     }
00341     for (std::vector<leg_type>::const_iterator it = support_leg_types.begin(); it != support_leg_types.end(); it++) {
00342         current_swing_time.at(*it) = (lcg_count + default_double_support_ratio_before * next_one_step_count) * dt;
00343     }
00344     for (std::vector<leg_type>::const_iterator it = swing_leg_types.begin(); it != swing_leg_types.end(); it++) {
00345         if (current_step_height > 0.0) {
00346             current_swing_time.at(*it) = tmp_current_swing_time;
00347         } else {
00348             current_swing_time.at(*it) = (lcg_count + default_double_support_ratio_before * next_one_step_count) * dt;
00349         }
00350     }
00351     //std::cerr << "sl " << support_leg << " " << current_swing_time[support_leg==RLEG?0:1] << " " << current_swing_time[support_leg==RLEG?1:0] << " " << tmp_current_swing_time << " " << lcg_count << std::endl;
00352   };
00353 
00354   double 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)
00355   {
00356       double tmp_ip_ratio;
00357       size_t current_count = one_step_count - lcg_count;
00358       if (thp.is_phase_starting(current_count, start_phase)) {
00359           toe_heel_interpolator->clear();
00360           toe_heel_interpolator->set(&start);
00361           //toe_heel_interpolator->go(&goal, thp.calc_phase_period(start_phase, goal_phase, dt));
00362           toe_heel_interpolator->setGoal(&goal, thp.calc_phase_period(start_phase, goal_phase, dt));
00363           toe_heel_interpolator->sync();
00364       }
00365       if (!toe_heel_interpolator->isEmpty()) {
00366           toe_heel_interpolator->get(&tmp_ip_ratio, true);
00367       } else {
00368           toe_heel_interpolator->get(&tmp_ip_ratio, false);
00369       }
00370       return tmp_ip_ratio;
00371   };
00372 
00373   void leg_coords_generator::modif_foot_coords_for_toe_heel_phase (coordinates& org_coords, const double _current_toe_angle, const double _current_heel_angle)
00374   {
00375       coordinates new_coords;
00376       size_t current_count = one_step_count - lcg_count;
00377       double dif_angle = 0.0;
00378       hrp::Vector3 ee_local_pivot_pos(hrp::Vector3(0,0,0));
00379       double first_goal_angle, second_goal_angle, first_pos_offset_x, second_pos_offset_x;
00380       if (use_toe_heel_auto_set) {
00381           first_goal_angle = set_value_according_to_toe_heel_type(current_src_toe_heel_type, _current_toe_angle, -1 * _current_heel_angle, 0);
00382           second_goal_angle = set_value_according_to_toe_heel_type(current_dst_toe_heel_type, _current_toe_angle, -1 * _current_heel_angle, 0);
00383           first_pos_offset_x = set_value_according_to_toe_heel_type(current_src_toe_heel_type, toe_pos_offset_x, heel_pos_offset_x, 0);
00384           second_pos_offset_x = set_value_according_to_toe_heel_type(current_dst_toe_heel_type, toe_pos_offset_x, heel_pos_offset_x, 0);
00385       } else {
00386           first_goal_angle = _current_toe_angle;
00387           second_goal_angle = -1 * _current_heel_angle;
00388           first_pos_offset_x = toe_pos_offset_x;
00389           second_pos_offset_x = heel_pos_offset_x;
00390       }
00391       if ( thp.is_between_phases(current_count, SOLE0, SOLE2TOE) ) {
00392           dif_angle = calc_interpolated_toe_heel_angle(SOLE0, SOLE2TOE, 0.0, first_goal_angle);
00393           ee_local_pivot_pos(0) = first_pos_offset_x;
00394       } else if ( thp.is_between_phases(current_count, SOLE2HEEL, HEEL2SOLE) ) {
00395           dif_angle = calc_interpolated_toe_heel_angle(SOLE2HEEL, HEEL2SOLE, second_goal_angle, 0.0);
00396           ee_local_pivot_pos(0) = second_pos_offset_x;
00397       } else if ( thp.is_between_phases(current_count, SOLE2TOE, SOLE2HEEL) ) {
00398           // If SOLE1 phase does not exist, interpolate toe => heel smoothly, without 0 velocity phase.
00399           if ( thp.is_no_SOLE1_phase() ) {
00400               dif_angle = calc_interpolated_toe_heel_angle(SOLE2TOE, SOLE2HEEL, first_goal_angle, second_goal_angle);
00401               double tmpd = (second_goal_angle-first_goal_angle);
00402               if (std::fabs(tmpd) > 1e-5) {
00403                   ee_local_pivot_pos(0) = (second_pos_offset_x - first_pos_offset_x) * (dif_angle - first_goal_angle) / tmpd + first_pos_offset_x;
00404               } else {
00405                   ee_local_pivot_pos(0) = first_pos_offset_x;
00406               }
00407           } else {
00408               if ( thp.is_between_phases(current_count, SOLE2TOE, TOE2SOLE) ) {
00409                   dif_angle = calc_interpolated_toe_heel_angle(SOLE2TOE, TOE2SOLE, first_goal_angle, 0.0);
00410                   ee_local_pivot_pos(0) = first_pos_offset_x;
00411               } else if ( thp.is_between_phases(current_count, SOLE1, SOLE2HEEL) ) {
00412                   dif_angle = calc_interpolated_toe_heel_angle(SOLE1, SOLE2HEEL, 0.0, second_goal_angle);
00413                   ee_local_pivot_pos(0) = second_pos_offset_x;
00414               }
00415           }
00416       }
00417       foot_dif_rot_angle = (dif_angle > 0.0 ? deg2rad(dif_angle) : 0.0);
00418       if (use_toe_joint && dif_angle > 0.0) dif_angle = 0.0;
00419       toe_heel_dif_angle = dif_angle;
00420       Eigen::AngleAxis<double> tmpr(deg2rad(dif_angle), hrp::Vector3::UnitY());
00421       rotm3times(new_coords.rot, org_coords.rot, tmpr.toRotationMatrix());
00422       new_coords.pos = org_coords.pos + org_coords.rot * ee_local_pivot_pos - new_coords.rot * ee_local_pivot_pos;
00423       org_coords = new_coords;
00424   };
00425 
00426   void leg_coords_generator::cycloid_midcoords (coordinates& ret, const coordinates& start,
00427                                                                 const coordinates& goal, const double height) const
00428   {
00429     cycloid_midpoint (ret.pos, swing_ratio, start.pos, goal.pos, height, default_top_ratio);
00430   };
00431 
00432   void leg_coords_generator::rectangle_midcoords (coordinates& ret, const coordinates& start,
00433                                                   const coordinates& goal, const double height, const size_t swing_trajectory_generator_idx)
00434   {
00435     rdtg[swing_trajectory_generator_idx].get_trajectory_point(ret.pos, hrp::Vector3(start.pos), hrp::Vector3(goal.pos), height);
00436   };
00437 
00438   void leg_coords_generator::stair_midcoords (coordinates& ret, const coordinates& start,
00439                                                               const coordinates& goal, const double height)
00440   {
00441     sdtg.get_trajectory_point(ret.pos, hrp::Vector3(start.pos), hrp::Vector3(goal.pos), height);
00442   };
00443 
00444   void leg_coords_generator::cycloid_delay_midcoords (coordinates& ret, const coordinates& start,
00445                                                       const coordinates& goal, const double height, const size_t swing_trajectory_generator_idx)
00446   {
00447     cdtg[swing_trajectory_generator_idx].get_trajectory_point(ret.pos, hrp::Vector3(start.pos), hrp::Vector3(goal.pos), height);
00448   };
00449 
00450   void leg_coords_generator::cycloid_delay_kick_midcoords (coordinates& ret, const coordinates& start,
00451                                                                       const coordinates& goal, const double height)
00452   {
00453     cdktg.set_start_rot(hrp::Matrix33(start.rot));
00454     cdktg.get_trajectory_point(ret.pos, hrp::Vector3(start.pos), hrp::Vector3(goal.pos), height);
00455   };
00456 
00457   void leg_coords_generator::cross_delay_midcoords (coordinates& ret, const coordinates& start,
00458                                                     const coordinates& goal, const double height, leg_type lr)
00459   {
00460     crdtg.set_swing_leg(lr);
00461     crdtg.get_trajectory_point(ret.pos, hrp::Vector3(start.pos), hrp::Vector3(goal.pos), height);
00462   };
00463 
00464   bool leg_coords_generator::is_same_footstep_nodes(const std::vector<step_node>& fns_1, const std::vector<step_node>& fns_2) const
00465   {
00466       bool matching_flag = true;
00467       if (fns_1.size() == fns_2.size()) {
00468           for (std::vector<step_node>::const_iterator it1 = fns_1.begin(); it1 != fns_1.end(); it1++) {
00469               std::vector<step_node>::const_iterator it2 = std::find_if(fns_2.begin(), fns_2.end(), (&boost::lambda::_1->* &step_node::l_r == it1->l_r));
00470               if (it2 == fns_2.end()) {
00471                   matching_flag = false;
00472                   break;
00473               }
00474           }
00475       } else {
00476           matching_flag = false;
00477       }
00478       return matching_flag;
00479   };
00480 
00481   void leg_coords_generator::calc_swing_support_mid_coords ()
00482   {
00483       std::vector<coordinates> swg_src_coords, swg_dst_coords,sup_coords;
00484       for (std::vector<step_node>::const_iterator it = swing_leg_src_steps.begin(); it != swing_leg_src_steps.end(); it++) {
00485           if (it->l_r == RLEG or it->l_r == LLEG) swg_src_coords.push_back(it->worldcoords);
00486       }
00487       for (std::vector<step_node>::const_iterator it = swing_leg_dst_steps.begin(); it != swing_leg_dst_steps.end(); it++) {
00488           if (it->l_r == RLEG or it->l_r == LLEG) swg_dst_coords.push_back(it->worldcoords);
00489       }
00490       for (std::vector<step_node>::const_iterator it = support_leg_steps.begin(); it != support_leg_steps.end(); it++) {
00491           if (it->l_r == RLEG or it->l_r == LLEG) sup_coords.push_back(it->worldcoords);
00492       }
00493       coordinates tmp_swg_src_mid, tmp_swg_dst_mid, tmp_swg_mid, tmp_sup_mid;
00494       const double rot_eps = 1e-5; // eps for mid_rot calculation
00495       if (swg_src_coords.size() > 0) multi_mid_coords(tmp_swg_src_mid, swg_src_coords, rot_eps);
00496       if (swg_dst_coords.size() > 0) multi_mid_coords(tmp_swg_dst_mid, swg_dst_coords, rot_eps);
00497       if (sup_coords.size() > 0) multi_mid_coords(tmp_sup_mid, sup_coords, rot_eps);
00498       if (lcg_count == one_step_count) {
00499           foot_midcoords_interpolator->clear();
00500           double tmp[foot_midcoords_interpolator->dimension()];
00501           for (size_t ii = 0; ii < 3; ii++) {
00502               tmp[ii] = tmp_swg_src_mid.pos(ii);
00503               tmp[ii+3] = 0;
00504           }
00505           foot_midcoords_interpolator->set(tmp);
00506           // set dst
00507           hrp::Matrix33 difrot(tmp_swg_src_mid.rot.transpose() * tmp_swg_dst_mid.rot);
00508           hrp::Vector3 tmpr = hrp::rpyFromRot(difrot);
00509           for (size_t ii = 0; ii < 3; ii++) {
00510               tmp[ii] = tmp_swg_dst_mid.pos(ii);
00511               tmp[ii+3] = tmpr(ii);
00512           }
00513           foot_midcoords_interpolator->setGoal(tmp, dt*one_step_count, true);
00514           foot_midcoords_interpolator->sync();
00515       } else {
00516           double tmp[foot_midcoords_interpolator->dimension()];
00517           hrp::Matrix33 difrot(tmp_swg_src_mid.rot.transpose() * tmp_swg_dst_mid.rot);
00518           hrp::Vector3 tmpr = hrp::rpyFromRot(difrot);
00519           for (size_t ii = 0; ii < 3; ii++) {
00520               tmp[ii] = tmp_swg_dst_mid.pos(ii);
00521               tmp[ii+3] = tmpr(ii);
00522           }
00523           foot_midcoords_interpolator->setGoal(tmp, dt*lcg_count, true);
00524           foot_midcoords_interpolator->sync();
00525       }
00526       if (!foot_midcoords_interpolator->isEmpty()) {
00527           double tmp[foot_midcoords_interpolator->dimension()];
00528           foot_midcoords_interpolator->get(tmp, true);
00529           hrp::Vector3 tmpr;
00530           for (size_t ii = 0; ii < 3; ii++) {
00531               tmp_swg_mid.pos(ii) = tmp[ii];
00532               tmpr(ii) = tmp[ii+3];
00533           }
00534           tmp_swg_mid.rot = tmp_swg_src_mid.rot * hrp::rotFromRpy(tmpr);
00535       } else {
00536           tmp_swg_mid = tmp_swg_dst_mid;
00537       }
00538       mid_coords(swing_support_midcoords, static_cast<double>(sup_coords.size()) / (swg_src_coords.size() + sup_coords.size()), tmp_swg_mid, tmp_sup_mid, rot_eps);
00539   };
00540 
00541   void leg_coords_generator::update_leg_steps (const std::vector< std::vector<step_node> >& fnsl, const double default_double_support_ratio_before, const double default_double_support_ratio_after, const toe_heel_type_checker& thtc)
00542   {
00543     // Get current swing coords, support coords, and support leg parameters
00544     calc_swing_support_params_from_footstep_nodes_list(fnsl);
00545     current_src_toe_heel_type = thtc.check_toe_heel_type_from_swing_support_coords(swing_leg_src_steps.front().worldcoords, support_leg_steps.front().worldcoords, toe_pos_offset_x, heel_pos_offset_x);
00546     current_dst_toe_heel_type = thtc.check_toe_heel_type_from_swing_support_coords(swing_leg_dst_steps.front().worldcoords, support_leg_steps.front().worldcoords, toe_pos_offset_x, heel_pos_offset_x);
00547     calc_swing_support_mid_coords ();
00548 
00549     calc_ratio_from_double_support_ratio(default_double_support_ratio_before, default_double_support_ratio_after);
00550     swing_leg_steps.clear();
00551     calc_current_swing_leg_steps(swing_leg_steps, current_step_height, current_toe_angle, current_heel_angle, default_double_support_ratio_before, default_double_support_ratio_after);
00552     if ( 1 <= lcg_count ) {
00553       lcg_count--;
00554     } else {
00555       //std::cerr << "gp " << footstep_index << std::endl;
00556       if (footstep_index < fnsl.size() - 1) {
00557         footstep_index++;
00558       }
00559       if (footstep_index < fnsl.size() - 1) {
00560         current_step_height = fnsl[footstep_index].front().step_height;
00561         current_toe_angle = fnsl[footstep_index].front().toe_angle;
00562         current_heel_angle = fnsl[footstep_index].front().heel_angle;
00563       } else {
00564         current_step_height = current_toe_angle = current_heel_angle = 0.0;
00565       }
00566       if (footstep_index < fnsl.size()) {
00567         one_step_count = static_cast<size_t>(fnsl[footstep_index].front().step_time/dt);
00568         thp.set_one_step_count(one_step_count);
00569       }
00570       if (footstep_index + 1 < fnsl.size()) {
00571         next_one_step_count = static_cast<size_t>(fnsl[footstep_index+1].front().step_time/dt);
00572       }
00573       lcg_count = one_step_count;
00574       switch (default_orbit_type) {
00575       case RECTANGLE:
00576           for (size_t i = 0; i < rdtg.size(); i++)
00577               rdtg[i].reset(one_step_count, default_double_support_ratio_before, default_double_support_ratio_after);
00578           break;
00579       case STAIR:
00580           sdtg.reset(one_step_count, default_double_support_ratio_before, default_double_support_ratio_after);
00581           break;
00582       case CYCLOIDDELAY:
00583           for (size_t i = 0; i < cdtg.size(); i++)
00584               cdtg[i].reset(one_step_count, default_double_support_ratio_before, default_double_support_ratio_after);
00585           break;
00586       case CYCLOIDDELAYKICK:
00587           cdktg.reset(one_step_count, default_double_support_ratio_before, default_double_support_ratio_after);
00588           break;
00589       case CROSS:
00590           crdtg.reset(one_step_count, default_double_support_ratio_before, default_double_support_ratio_after);
00591           break;
00592       default:
00593           break;
00594       }
00595     }
00596   };
00597 
00598   /* member function implementation for gait_generator */
00599   void gait_generator::initialize_gait_parameter (const hrp::Vector3& cog,
00600                                                   const std::vector<step_node>& initial_support_leg_steps,
00601                                                   const std::vector<step_node>& initial_swing_leg_dst_steps,
00602                                                   const double delay)
00603   {
00604     /* clear all gait_parameter */
00605     size_t one_step_len = footstep_nodes_list.front().front().step_time / dt;
00606     finalize_count = 0;
00607     for (std::vector<step_node>::iterator it_fns = footstep_nodes_list.front().begin(); it_fns != footstep_nodes_list.front().end(); it_fns++) {
00608         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++) {
00609             if (it_fns->l_r == it_init->l_r) {
00610                 /* initial_swing_leg_dst_steps has dummy step_height, step_time, toe_angle and heel_angle. */
00611                 it_fns->worldcoords = it_init->worldcoords;
00612                 break;
00613             }
00614         }
00615     }
00616     // get initial_foot_mid_coords
00617     std::vector<coordinates> cv;
00618     for (size_t i = 0; i < initial_support_leg_steps.size(); i++) {
00619         cv.push_back(initial_support_leg_steps[i].worldcoords);
00620     }
00621     for (size_t i = 0; i < initial_swing_leg_dst_steps.size(); i++) {
00622         cv.push_back(initial_swing_leg_dst_steps[i].worldcoords);
00623     }
00624     multi_mid_coords(initial_foot_mid_coords, cv);
00625     // rg+lcg initialization
00626     rg.reset(one_step_len);
00627     rg.push_refzmp_from_footstep_nodes_for_dual(footstep_nodes_list.front(), initial_support_leg_steps, initial_swing_leg_dst_steps);
00628     if ( preview_controller_ptr != NULL ) {
00629       delete preview_controller_ptr;
00630       preview_controller_ptr = NULL;
00631     }
00632     //preview_controller_ptr = new preview_dynamics_filter<preview_control>(dt, cog(2) - refzmp_cur_list[0](2), refzmp_cur_list[0]);
00633     preview_controller_ptr = new preview_dynamics_filter<extended_preview_control>(dt, cog(2) - rg.get_refzmp_cur()(2), rg.get_refzmp_cur(), gravitational_acceleration);
00634     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);
00635     /* make another */
00636     lcg.set_swing_support_steps_list(footstep_nodes_list);
00637     for (size_t i = 1; i < footstep_nodes_list.size()-1; i++) {
00638         std::vector<step_node> tmp_swing_leg_src_steps;
00639         lcg.calc_swing_leg_src_steps(tmp_swing_leg_src_steps, footstep_nodes_list, i);
00640         toe_heel_types tht(thtc.check_toe_heel_type_from_swing_support_coords(tmp_swing_leg_src_steps.front().worldcoords, lcg.get_support_leg_steps_idx(i).front().worldcoords, lcg.get_toe_pos_offset_x(), lcg.get_heel_pos_offset_x()),
00641                            thtc.check_toe_heel_type_from_swing_support_coords(lcg.get_swing_leg_dst_steps_idx(i).front().worldcoords, lcg.get_support_leg_steps_idx(i).front().worldcoords, lcg.get_toe_pos_offset_x(), lcg.get_heel_pos_offset_x()));
00642         rg.push_refzmp_from_footstep_nodes_for_single(footstep_nodes_list.at(i), lcg.get_support_leg_steps_idx(i), tht);
00643     }
00644     rg.push_refzmp_from_footstep_nodes_for_dual(footstep_nodes_list.back(),
00645                                                 lcg.get_support_leg_steps_idx(footstep_nodes_list.size()-1),
00646                                                 lcg.get_swing_leg_dst_steps_idx(footstep_nodes_list.size()-1));
00647     emergency_flg = IDLING;
00648   };
00649 
00650   bool gait_generator::proc_one_tick ()
00651   {
00652     solved = false;
00653     /* update refzmp */
00654     if (emergency_flg == EMERGENCY_STOP && lcg.get_footstep_index() > 0) {
00655         leg_type cur_leg = footstep_nodes_list[lcg.get_footstep_index()].front().l_r;
00656         leg_type first_step = overwritable_footstep_index_offset % 2 == 0 ? cur_leg : (cur_leg == RLEG ? LLEG : RLEG);
00657 
00658         overwrite_footstep_nodes_list.push_back(boost::assign::list_of(step_node(first_step, footstep_nodes_list[get_overwritable_index() - 2].front().worldcoords, 0, default_step_time, 0, 0)));
00659         overwrite_footstep_nodes_list.push_back(boost::assign::list_of(step_node(first_step==RLEG?LLEG:RLEG, footstep_nodes_list[get_overwritable_index() - 1].front().worldcoords, 0, default_step_time, 0, 0)));
00660         overwrite_footstep_nodes_list.push_back(boost::assign::list_of(step_node(first_step, footstep_nodes_list[get_overwritable_index() - 2].front().worldcoords, 0, default_step_time, 0, 0)));
00661 
00662         overwrite_refzmp_queue(overwrite_footstep_nodes_list);
00663         overwrite_footstep_nodes_list.clear();
00664         emergency_flg = STOPPING;
00665     } else if ( lcg.get_lcg_count() == get_overwrite_check_timing() ) {
00666       if (velocity_mode_flg != VEL_IDLING && lcg.get_footstep_index() > 0) {
00667         std::vector< std::vector<step_node> > cv;
00668         calc_next_coords_velocity_mode(cv, get_overwritable_index(),
00669                                        (overwritable_footstep_index_offset == 0 ? 4 : 3) // Why?
00670                                        );
00671         if (velocity_mode_flg == VEL_ENDING) velocity_mode_flg = VEL_IDLING;
00672         std::vector<leg_type> first_overwrite_leg;
00673         for (size_t i = 0; i < footstep_nodes_list[get_overwritable_index()].size(); i++) {
00674             first_overwrite_leg.push_back(footstep_nodes_list[get_overwritable_index()].at(i).l_r);
00675         }
00676         for (size_t i = 0; i < cv.size(); i++) {
00677             std::vector<step_node> tmp_fsn;
00678             for (size_t j = 0; j < cv.at(i).size(); j++) {
00679                 cv.at(i).at(j).worldcoords.pos += modified_d_footstep;
00680                 tmp_fsn.push_back(step_node(cv.at(i).at(j).l_r, cv.at(i).at(j).worldcoords,
00681                                             lcg.get_default_step_height(), default_step_time, lcg.get_toe_angle(), lcg.get_heel_angle()));
00682             }
00683             overwrite_footstep_nodes_list.push_back(tmp_fsn);
00684         }
00685         overwrite_refzmp_queue(overwrite_footstep_nodes_list);
00686         overwrite_footstep_nodes_list.clear();
00687       } else if ( !overwrite_footstep_nodes_list.empty() && // If overwrite_footstep_node_list exists
00688                   (lcg.get_footstep_index() < footstep_nodes_list.size()-1) &&  // If overwrite_footstep_node_list is specified and current footstep is not last footstep.
00689                   get_overwritable_index() == overwrite_footstep_index ) {
00690         overwrite_refzmp_queue(overwrite_footstep_nodes_list);
00691         overwrite_footstep_nodes_list.clear();
00692       }
00693     }
00694     // limit stride
00695     if (use_stride_limitation && lcg.get_footstep_index() > 0 && lcg.get_footstep_index() < footstep_nodes_list.size()-overwritable_footstep_index_offset-2 &&
00696         (overwritable_footstep_index_offset == 0 || lcg.get_lcg_count() == get_overwrite_check_timing())) {
00697       if (lcg.get_footstep_index() == footstep_nodes_list.size()-overwritable_footstep_index_offset-3) {
00698         hrp::Vector3 orig_footstep_pos = footstep_nodes_list[get_overwritable_index()].front().worldcoords.pos;
00699         limit_stride(footstep_nodes_list[get_overwritable_index()].front(), footstep_nodes_list[get_overwritable_index()-1].front(), overwritable_stride_limitation);
00700         for (size_t i = get_overwritable_index() + 1; i < footstep_nodes_list.size(); i++) {
00701           footstep_nodes_list[i].front().worldcoords.pos -= orig_footstep_pos - footstep_nodes_list[get_overwritable_index()].front().worldcoords.pos;
00702         }
00703       } else {
00704         limit_stride(footstep_nodes_list[get_overwritable_index()].front(), footstep_nodes_list[get_overwritable_index()-1].front(), overwritable_stride_limitation);
00705       }
00706       overwrite_footstep_nodes_list.insert(overwrite_footstep_nodes_list.end(), footstep_nodes_list.begin()+get_overwritable_index(), footstep_nodes_list.end());
00707       overwrite_refzmp_queue(overwrite_footstep_nodes_list);
00708       overwrite_footstep_nodes_list.clear();
00709     }
00710     // modify footsteps based on diff_cp
00711     if(modify_footsteps) modify_footsteps_for_recovery();
00712 
00713     if ( !solved ) {
00714       hrp::Vector3 rzmp;
00715       std::vector<hrp::Vector3> sfzos;
00716       bool refzmp_exist_p = rg.get_current_refzmp(rzmp, sfzos, default_double_support_ratio_before, default_double_support_ratio_after, default_double_support_static_ratio_before, default_double_support_static_ratio_after);
00717       if (!refzmp_exist_p) {
00718         finalize_count++;
00719         rzmp = prev_que_rzmp;
00720         sfzos = prev_que_sfzos;
00721       } else {
00722         prev_que_rzmp = rzmp;
00723         prev_que_sfzos = sfzos;
00724       }
00725       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));
00726     }
00727 
00728     rg.update_refzmp();
00729     // { // debug
00730     //   double cart_zmp[3];
00731     //   preview_controller_ptr->get_cart_zmp(cart_zmp);
00732     //   std::cerr << "(list " << std::endl;
00733     //   std::cerr << ":cog "; print_vector(std::cerr, cog);
00734     //   std::cerr << ":refzmp "; print_vector(std::cerr, refzmp);
00735     //   std::cerr << ":cart-zmp "; print_vector(std::cerr, cart_zmp, 3);
00736     //   std::cerr << ")" << std::endl;
00737     // }
00738 
00739     /* update swing_leg_coords, support_leg_coords */
00740     if ( solved ) {
00741       lcg.update_leg_steps(footstep_nodes_list, default_double_support_ratio_swing_before, default_double_support_ratio_swing_after, thtc);
00742     } else if (finalize_count>0) {
00743       lcg.clear_interpolators();
00744     }
00745     return solved;
00746   };
00747 
00748   void gait_generator::limit_stride (step_node& cur_fs, const step_node& prev_fs, const double (&limit)[5]) const
00749   {
00750     // limit[5] = {forward, outside, theta, backward, inside}
00751     leg_type cur_leg = cur_fs.l_r;
00752     // prev_fs frame
00753     cur_fs.worldcoords.pos = prev_fs.worldcoords.rot.transpose() * (cur_fs.worldcoords.pos - prev_fs.worldcoords.pos);
00754     double stride_r = std::pow(cur_fs.worldcoords.pos(0), 2.0) + std::pow(cur_fs.worldcoords.pos(1) + footstep_param.leg_default_translate_pos[cur_leg == LLEG ? RLEG : LLEG](1) - footstep_param.leg_default_translate_pos[cur_leg](1), 2.0);
00755     // front, rear, outside limitation
00756     double stride_r_limit = std::pow(std::max(limit[cur_fs.worldcoords.pos(0) >= 0 ? 0 : 3], limit[1] - limit[4]), 2.0);
00757     if (stride_r > stride_r_limit && (cur_leg == LLEG ? 1 : -1) * cur_fs.worldcoords.pos(1) > footstep_param.leg_default_translate_pos[LLEG](1) - footstep_param.leg_default_translate_pos[RLEG](1)) {
00758       cur_fs.worldcoords.pos(0) *= sqrt(stride_r_limit / stride_r);
00759       cur_fs.worldcoords.pos(1) = footstep_param.leg_default_translate_pos[cur_leg](1) - footstep_param.leg_default_translate_pos[cur_leg == LLEG ? RLEG : LLEG](1) +
00760                                   sqrt(stride_r_limit / stride_r) * (cur_fs.worldcoords.pos(1) + footstep_param.leg_default_translate_pos[cur_leg == LLEG ? RLEG : LLEG](1) - footstep_param.leg_default_translate_pos[cur_leg](1));
00761     }
00762     if (cur_fs.worldcoords.pos(0) > limit[0]) cur_fs.worldcoords.pos(0) = limit[0];
00763     if (cur_fs.worldcoords.pos(0) < -1 * limit[0]) cur_fs.worldcoords.pos(0) = -1 * limit[3];
00764     if ((cur_leg == LLEG ? 1 : -1) * cur_fs.worldcoords.pos(1) > limit[1]) cur_fs.worldcoords.pos(1) = (cur_leg == LLEG ? 1 : -1) * limit[1];
00765     // inside limitation
00766     std::vector<double> cur_leg_vertices_y;
00767     cur_leg_vertices_y.reserve(4);
00768     cur_leg_vertices_y.push_back((cur_fs.worldcoords.pos + prev_fs.worldcoords.rot.transpose() * cur_fs.worldcoords.rot * hrp::Vector3(leg_margin[0], (cur_leg == LLEG ? 1 : -1) * leg_margin[2], 0.0))(1));
00769     cur_leg_vertices_y.push_back((cur_fs.worldcoords.pos + prev_fs.worldcoords.rot.transpose() * cur_fs.worldcoords.rot * hrp::Vector3(leg_margin[0], (cur_leg == LLEG ? -1 : 1) * leg_margin[3], 0.0))(1));
00770     cur_leg_vertices_y.push_back((cur_fs.worldcoords.pos + prev_fs.worldcoords.rot.transpose() * cur_fs.worldcoords.rot * hrp::Vector3(-1 * leg_margin[1], (cur_leg == LLEG ? 1 : -1) * leg_margin[2], 0.0))(1));
00771     cur_leg_vertices_y.push_back((cur_fs.worldcoords.pos + prev_fs.worldcoords.rot.transpose() * cur_fs.worldcoords.rot * hrp::Vector3(-1 * leg_margin[1], (cur_leg == LLEG ? -1 : 1) * leg_margin[3], 0.0))(1));
00772     if (cur_leg == LLEG) {
00773       if (*std::min_element(cur_leg_vertices_y.begin(), cur_leg_vertices_y.end()) < limit[4]) cur_fs.worldcoords.pos(1) += limit[4] - *std::min_element(cur_leg_vertices_y.begin(), cur_leg_vertices_y.end());
00774     } else {
00775       if (*std::max_element(cur_leg_vertices_y.begin(), cur_leg_vertices_y.end()) > -1 * limit[4]) cur_fs.worldcoords.pos(1) += -1 * limit[4] - *std::max_element(cur_leg_vertices_y.begin(), cur_leg_vertices_y.end());
00776     }
00777     // world frame
00778     cur_fs.worldcoords.pos = prev_fs.worldcoords.pos + prev_fs.worldcoords.rot * cur_fs.worldcoords.pos;
00779   };
00780 
00781   void gait_generator::modify_footsteps_for_recovery ()
00782   {
00783     if (isfinite(diff_cp(0)) && isfinite(diff_cp(1))) {
00784       // calculate diff_cp
00785       hrp::Vector3 tmp_diff_cp;
00786       for (size_t i = 0; i < 2; i++) {
00787         if (std::fabs(diff_cp(i)) > cp_check_margin[i]) {
00788           is_emergency_walking[i] = true;
00789           tmp_diff_cp(i) = diff_cp(i) - cp_check_margin[i] * diff_cp(i)/std::fabs(diff_cp(i));
00790         } else {
00791           is_emergency_walking[i] = false;
00792         }
00793       }
00794       if (lcg.get_footstep_index() > 0 && lcg.get_footstep_index() < footstep_nodes_list.size()-2) {
00795         // calculate sum of preview_f
00796         static double preview_f_sum;
00797         if (lcg.get_lcg_count() == static_cast<size_t>(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * 1.0) - 1) {
00798           preview_f_sum = preview_controller_ptr->get_preview_f(preview_controller_ptr->get_delay());
00799           for (size_t i = preview_controller_ptr->get_delay()-1; i >= lcg.get_lcg_count()+1; i--) {
00800             preview_f_sum += preview_controller_ptr->get_preview_f(i);
00801           }
00802           modified_d_footstep = hrp::Vector3::Zero();
00803         }
00804         if (lcg.get_lcg_count() <= preview_controller_ptr->get_delay()) {
00805           preview_f_sum += preview_controller_ptr->get_preview_f(lcg.get_lcg_count());
00806         }
00807         // calculate modified footstep position
00808         double preview_db = 1/6.0 * dt * dt * dt + 1/2.0 * dt * dt * 1/std::sqrt(gravitational_acceleration / (cog(2) - refzmp(2)));
00809         hrp::Vector3 d_footstep = -1/preview_f_sum * 1/preview_db * footstep_modification_gain * tmp_diff_cp;
00810         d_footstep(2) = 0.0;
00811         // overwrite footsteps
00812         if (lcg.get_lcg_count() <= static_cast<size_t>(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * 1.0) - 1 &&
00813             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 &&
00814             !(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])) {
00815           // stride limitation check
00816           hrp::Vector3 orig_footstep_pos = footstep_nodes_list[get_overwritable_index()].front().worldcoords.pos;
00817           for (size_t i = 0; i < 2; i++) {
00818             if (is_emergency_walking[i]) footstep_nodes_list[get_overwritable_index()].front().worldcoords.pos(i) += d_footstep(i);
00819           }
00820           limit_stride(footstep_nodes_list[get_overwritable_index()].front(), footstep_nodes_list[get_overwritable_index()-1].front(), overwritable_stride_limitation);
00821           d_footstep = footstep_nodes_list[get_overwritable_index()].front().worldcoords.pos - orig_footstep_pos;
00822           for (size_t i = lcg.get_footstep_index()+1; i < footstep_nodes_list.size(); i++) {
00823             footstep_nodes_list[i].front().worldcoords.pos += d_footstep;
00824           }
00825           if (is_emergency_walking[0] || is_emergency_walking[1]) {
00826             overwrite_footstep_nodes_list.insert(overwrite_footstep_nodes_list.end(), footstep_nodes_list.begin()+lcg.get_footstep_index(), footstep_nodes_list.end());
00827             // overwrite zmp
00828             overwrite_refzmp_queue(overwrite_footstep_nodes_list);
00829             overwrite_footstep_nodes_list.clear();
00830             modified_d_footstep += d_footstep;
00831           }
00832         }
00833       } else {
00834         modified_d_footstep = hrp::Vector3::Zero();
00835       }
00836     }
00837   }
00838 
00839   /* generate vector of step_node from :go-pos params
00840    *  x, y and theta are simply divided by using stride params
00841    *  unit system -> x [mm], y [mm], theta [deg]
00842    */
00843   bool gait_generator::go_pos_param_2_footstep_nodes_list (const double goal_x, const double goal_y, const double goal_theta, /* [mm] [mm] [deg] */
00844                                                            const std::vector<coordinates>& initial_support_legs_coords, coordinates start_ref_coords,
00845                                                            const std::vector<leg_type>& initial_support_legs,
00846                                                            std::vector< std::vector<step_node> >& new_footstep_nodes_list,
00847                                                            const bool is_initialize)
00848   {
00849     // Get overwrite footstep index
00850     size_t overwritable_fs_index = 0;
00851     if (!is_initialize) {
00852         if (lcg.get_footstep_index() <= get_overwrite_check_timing()) { // ending half
00853             overwritable_fs_index = get_overwritable_index()+1;
00854         } else { // starting half
00855             overwritable_fs_index = get_overwritable_index();
00856         }
00857     }
00858     // Check overwritable_fs_index
00859     if (overwritable_fs_index > footstep_nodes_list.size()-1) return false;
00860     go_pos_param_2_footstep_nodes_list_core (goal_x, goal_y, goal_theta,
00861                                              initial_support_legs_coords, start_ref_coords, initial_support_legs,
00862                                              new_footstep_nodes_list, is_initialize, overwritable_fs_index);
00863     //   For Last double support period
00864     if (is_initialize) {
00865         clear_footstep_nodes_list();
00866         footstep_nodes_list = new_footstep_nodes_list;
00867     } else {
00868         set_overwrite_foot_steps_list(new_footstep_nodes_list);
00869         set_overwrite_foot_step_index(overwritable_fs_index);
00870     }
00871     print_footstep_nodes_list();
00872     return true;
00873   };
00874 
00875   void gait_generator::go_pos_param_2_footstep_nodes_list_core (const double goal_x, const double goal_y, const double goal_theta, /* [mm] [mm] [deg] */
00876                                                                 const std::vector<coordinates>& initial_support_legs_coords, coordinates start_ref_coords,
00877                                                                 const std::vector<leg_type>& initial_support_legs,
00878                                                                 std::vector< std::vector<step_node> >& new_footstep_nodes_list,
00879                                                                 const bool is_initialize, const size_t overwritable_fs_index) const
00880   {
00881     // Calc goal ref
00882     coordinates goal_ref_coords;
00883     if (is_initialize) {
00884         goal_ref_coords = start_ref_coords;
00885     } else {
00886         goal_ref_coords = initial_foot_mid_coords;
00887         step_node tmpfs = footstep_nodes_list[overwritable_fs_index-1].front();
00888         start_ref_coords = tmpfs.worldcoords;
00889         start_ref_coords.pos += start_ref_coords.rot * hrp::Vector3(-1*footstep_param.leg_default_translate_pos[tmpfs.l_r]);
00890     }
00891     goal_ref_coords.pos += goal_ref_coords.rot * hrp::Vector3(goal_x, goal_y, 0.0);
00892     goal_ref_coords.rotate(deg2rad(goal_theta), hrp::Vector3(0,0,1));
00893     std::cerr << "start ref coords" << std::endl;
00894     std::cerr << "  pos =" << std::endl;
00895     std::cerr << start_ref_coords.pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
00896     std::cerr << "  rot =" << std::endl;
00897     std::cerr << start_ref_coords.rot.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
00898     std::cerr << "goal ref midcoords" << std::endl;
00899     std::cerr << "  pos =" << std::endl;
00900     std::cerr << goal_ref_coords.pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
00901     std::cerr << "  rot =" << std::endl;
00902     std::cerr << goal_ref_coords.rot.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
00903 
00904     /* initialize */
00905     if (is_initialize) {
00906         // For initial double support period
00907         std::vector<step_node> initial_footstep_nodes;
00908         for (size_t i = 0; i < initial_support_legs.size(); i++) {
00909             initial_footstep_nodes.push_back(step_node(initial_support_legs.at(i), initial_support_legs_coords.at(i), 0, default_step_time, 0, 0));
00910         }
00911         new_footstep_nodes_list.push_back(initial_footstep_nodes);
00912     } else {
00913         new_footstep_nodes_list.push_back(footstep_nodes_list[overwritable_fs_index]);
00914     }
00915 
00916     /* footstep generation loop */
00917     hrp::Vector3 dp, dr;
00918     start_ref_coords.difference(dp, dr, goal_ref_coords);
00919     dp = start_ref_coords.rot.transpose() * dp;
00920     dr = start_ref_coords.rot.transpose() * dr;
00921     while ( !(eps_eq(std::sqrt(dp(0)*dp(0)+dp(1)*dp(1)), 0.0, 1e-3*0.1) && eps_eq(dr(2), 0.0, deg2rad(0.5))) ) {
00922       velocity_mode_parameter cur_vel_param;
00923       cur_vel_param.set(dp(0)/default_step_time, dp(1)/default_step_time, rad2deg(dr(2))/default_step_time);
00924       append_footstep_list_velocity_mode(new_footstep_nodes_list, cur_vel_param);
00925       start_ref_coords = new_footstep_nodes_list.back().front().worldcoords;
00926       start_ref_coords.pos += start_ref_coords.rot * hrp::Vector3(footstep_param.leg_default_translate_pos[new_footstep_nodes_list.back().front().l_r] * -1.0);
00927       start_ref_coords.difference(dp, dr, goal_ref_coords);
00928       dp = start_ref_coords.rot.transpose() * dp;
00929       dr = start_ref_coords.rot.transpose() * dr;
00930     }
00931     for (size_t i = 0; i < optional_go_pos_finalize_footstep_num; i++) {
00932         append_go_pos_step_nodes(start_ref_coords, calc_counter_leg_types_from_footstep_nodes(new_footstep_nodes_list.back(), all_limbs), new_footstep_nodes_list);
00933     }
00934 
00935     /* finalize */
00936     //   Align last foot
00937     append_go_pos_step_nodes(start_ref_coords, calc_counter_leg_types_from_footstep_nodes(new_footstep_nodes_list.back(), all_limbs), new_footstep_nodes_list);
00938     //   Check align
00939     coordinates final_step_coords1 = new_footstep_nodes_list[new_footstep_nodes_list.size()-2].front().worldcoords; // Final coords in footstep_node_list
00940     coordinates final_step_coords2 = start_ref_coords; // Final coords calculated from start_ref_coords + translate pos
00941     final_step_coords2.pos += final_step_coords2.rot * hrp::Vector3(footstep_param.leg_default_translate_pos[new_footstep_nodes_list[new_footstep_nodes_list.size()-2].front().l_r]);
00942     final_step_coords1.difference(dp, dr, final_step_coords2);
00943     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
00944         append_go_pos_step_nodes(start_ref_coords, calc_counter_leg_types_from_footstep_nodes(new_footstep_nodes_list.back(), all_limbs), new_footstep_nodes_list);
00945     }
00946     //   For Last double support period
00947     if (is_initialize) {
00948         append_finalize_footstep(new_footstep_nodes_list);
00949     }
00950     return;
00951   };
00952 
00953   void gait_generator::go_single_step_param_2_footstep_nodes_list (const double goal_x, const double goal_y, const double goal_z, const double goal_theta,
00954                                                              const std::string& tmp_swing_leg,
00955                                                              const coordinates& _support_leg_coords)
00956   {
00957     leg_type _swing_leg = (tmp_swing_leg == "rleg") ? RLEG : LLEG;
00958     step_node sn0((_swing_leg == RLEG) ? LLEG : RLEG, _support_leg_coords, lcg.get_default_step_height(), default_step_time, lcg.get_toe_angle(), lcg.get_heel_angle());
00959     footstep_nodes_list.push_back(boost::assign::list_of(sn0));
00960     step_node sn1(_swing_leg, _support_leg_coords, lcg.get_default_step_height(), default_step_time, lcg.get_toe_angle(), lcg.get_heel_angle());
00961     hrp::Vector3 trs(2.0 * footstep_param.leg_default_translate_pos[_swing_leg] + hrp::Vector3(goal_x, goal_y, goal_z));
00962     sn1.worldcoords.pos += sn1.worldcoords.rot * trs;
00963     sn1.worldcoords.rotate(deg2rad(goal_theta), hrp::Vector3(0,0,1));
00964     footstep_nodes_list.push_back(boost::assign::list_of(sn1));
00965     footstep_nodes_list.push_back(boost::assign::list_of(sn0));
00966   };
00967 
00968   void gait_generator::initialize_velocity_mode (const coordinates& _ref_coords,
00969                                                  const double vel_x, const double vel_y, const double vel_theta,
00970                                                  const std::vector<leg_type>& current_legs)
00971   {
00972     velocity_mode_flg = VEL_DOING;
00973     /* initialize */
00974     clear_footstep_nodes_list();
00975     set_velocity_param (vel_x, vel_y, vel_theta);
00976     append_go_pos_step_nodes(_ref_coords, current_legs);
00977     append_footstep_list_velocity_mode();
00978     append_footstep_list_velocity_mode();
00979     append_footstep_list_velocity_mode();
00980   };
00981 
00982   void gait_generator::finalize_velocity_mode ()
00983   {
00984     if (velocity_mode_flg == VEL_DOING) velocity_mode_flg = VEL_ENDING;
00985   };
00986 
00987   void gait_generator::calc_ref_coords_trans_vector_velocity_mode (coordinates& ref_coords, hrp::Vector3& trans, double& dth, const std::vector<step_node>& sup_fns, const velocity_mode_parameter& cur_vel_param) const
00988   {
00989     ref_coords = sup_fns.front().worldcoords;
00990     hrp::Vector3 tmpv(footstep_param.leg_default_translate_pos[sup_fns.front().l_r] * -1.0); /* not fair to every support legs */
00991     ref_coords.pos += ref_coords.rot * tmpv;
00992     double dx = cur_vel_param.velocity_x + offset_vel_param.velocity_x, dy = cur_vel_param.velocity_y + offset_vel_param.velocity_y;
00993     dth = cur_vel_param.velocity_theta + offset_vel_param.velocity_theta;
00994     //std::cerr << "Before limit dx " << dx << " dy " << dy << " dth " << dth << std::endl;
00995     /* velocity limitation by stride parameters <- this should be based on footstep candidates */
00996     if (default_stride_limitation_type == SQUARE) {
00997       dth = std::max(-1 * footstep_param.stride_outside_theta / default_step_time, std::min(footstep_param.stride_outside_theta / default_step_time, dth));
00998     } else if (default_stride_limitation_type == CIRCLE) {
00999       dth = std::max(-1 * stride_limitation_for_circle_type[2] / default_step_time, std::min(stride_limitation_for_circle_type[2] / default_step_time, dth));
01000     }
01001     if (default_stride_limitation_type == SQUARE) {
01002       dx  = std::max(-1 * footstep_param.stride_bwd_x / default_step_time, std::min(footstep_param.stride_fwd_x / default_step_time, dx ));
01003       dy  = std::max(-1 * footstep_param.stride_outside_y     / default_step_time, std::min(footstep_param.stride_outside_y     / default_step_time, dy ));
01004       /* inside step limitation */
01005       if (use_inside_step_limitation) {
01006         if (dy > 0) {
01007             // If dy>0 (== leftward step) and LLEG/LARM support, do inside limitation
01008             if (std::count_if(sup_fns.begin(), sup_fns.end(), (&boost::lambda::_1->* &step_node::l_r == LLEG || &boost::lambda::_1->* &step_node::l_r == LARM)) > 0) {
01009                 dy  = std::min(footstep_param.stride_inside_y     / default_step_time, dy);
01010             }
01011         } else {
01012             // If dy<=0 (== rightward step) and RLEG/RARM support, do inside limitation
01013             if (std::count_if(sup_fns.begin(), sup_fns.end(), (&boost::lambda::_1->* &step_node::l_r == RLEG || &boost::lambda::_1->* &step_node::l_r == RARM)) > 0) {
01014                 dy  = std::max(-1 * footstep_param.stride_inside_y     / default_step_time, dy);
01015             }
01016         }
01017         if (dth > 0) {
01018             // If dth>0 (== leftward turn step) and LLEG/LARM support, do inside limitation
01019             if (std::count_if(sup_fns.begin(), sup_fns.end(), (&boost::lambda::_1->* &step_node::l_r == LLEG || &boost::lambda::_1->* &step_node::l_r == LARM)) > 0) {
01020                 dth = std::min(footstep_param.stride_inside_theta / default_step_time, dth);
01021             }
01022         } else {
01023             // If dth<=0 (== rightward turn step) and RLEG/RARM support, do inside limitation
01024             if (std::count_if(sup_fns.begin(), sup_fns.end(), (&boost::lambda::_1->* &step_node::l_r == RLEG || &boost::lambda::_1->* &step_node::l_r == RARM)) > 0) {
01025                 dth = std::max(-1 * footstep_param.stride_inside_theta / default_step_time, dth);
01026             }
01027         }
01028       }
01029     }
01030     //std::cerr << "After Limit dx " << dx << " dy " << dy << " dth " << dth << std::endl;
01031     trans = hrp::Vector3(dx * default_step_time, dy * default_step_time, 0);
01032     dth = deg2rad(dth * default_step_time);
01033   };
01034 
01035   void gait_generator::append_footstep_list_velocity_mode ()
01036   {
01037       append_footstep_list_velocity_mode(footstep_nodes_list, vel_param);
01038   };
01039 
01040   void gait_generator::append_footstep_list_velocity_mode (std::vector< std::vector<step_node> >& _footstep_nodes_list, const velocity_mode_parameter& cur_vel_param) const
01041   {
01042     coordinates ref_coords;
01043     hrp::Vector3 trans;
01044     double dth;
01045     calc_ref_coords_trans_vector_velocity_mode(ref_coords, trans, dth, _footstep_nodes_list.back(), cur_vel_param);
01046 
01047     ref_coords.pos += ref_coords.rot * trans;
01048     ref_coords.rotate(dth, hrp::Vector3(0,0,1));
01049     append_go_pos_step_nodes(ref_coords, calc_counter_leg_types_from_footstep_nodes(_footstep_nodes_list.back(), all_limbs), _footstep_nodes_list);
01050     if (default_stride_limitation_type == CIRCLE) limit_stride(_footstep_nodes_list[_footstep_nodes_list.size()-1].front(), _footstep_nodes_list[_footstep_nodes_list.size()-2].front(), stride_limitation_for_circle_type);
01051   };
01052 
01053   void gait_generator::calc_next_coords_velocity_mode (std::vector< std::vector<step_node> >& ret_list, const size_t idx, const size_t future_step_num)
01054   {
01055     coordinates ref_coords;
01056     hrp::Vector3 trans;
01057     double dth;
01058     calc_ref_coords_trans_vector_velocity_mode(ref_coords, trans, dth, footstep_nodes_list[idx-1], vel_param);
01059 
01060     std::vector<leg_type> cur_sup_legs, next_sup_legs;
01061     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);
01062     next_sup_legs = calc_counter_leg_types_from_footstep_nodes(footstep_nodes_list[idx-1], all_limbs);
01063 
01064     for (size_t i = 0; i < future_step_num; i++) {
01065       std::vector<step_node> ret;
01066       std::vector<leg_type> forcused_sup_legs;
01067       switch( i % 2) {
01068       case 0: forcused_sup_legs = next_sup_legs; break;
01069       case 1: forcused_sup_legs = cur_sup_legs; break;
01070       }
01071       if ( velocity_mode_flg != VEL_ENDING ) {
01072           ref_coords.pos += ref_coords.rot * trans;
01073           ref_coords.rotate(dth, hrp::Vector3(0,0,1));
01074       }
01075       for (size_t j = 0; j < forcused_sup_legs.size(); j++) {
01076           ret.push_back(step_node(forcused_sup_legs.at(j), ref_coords, 0, 0, 0, 0));
01077           ret[j].worldcoords.pos += ret[j].worldcoords.rot * footstep_param.leg_default_translate_pos[forcused_sup_legs.at(j)];
01078           if (default_stride_limitation_type == CIRCLE) limit_stride(ret[j], (i == 0 ? footstep_nodes_list[idx-1].at(j) : ret_list[i -1].at(j)), stride_limitation_for_circle_type);
01079       }
01080       ret_list.push_back(ret);
01081     }
01082   };
01083 
01084   void gait_generator::overwrite_refzmp_queue(const std::vector< std::vector<step_node> >& fnsl)
01085   {
01086     size_t idx = get_overwritable_index();
01087     footstep_nodes_list.erase(footstep_nodes_list.begin()+idx, footstep_nodes_list.end());
01088 
01089     /* add new next steps ;; the number of next steps is fnsl.size() */
01090     footstep_nodes_list.insert(footstep_nodes_list.end(), fnsl.begin(), fnsl.end());
01091 
01092     /* Update lcg */
01093     lcg.set_swing_support_steps_list(footstep_nodes_list);
01094 
01095     /* Update refzmp_generator */
01096     /*   Remove refzmp after idx for allocation of new refzmp by push_refzmp_from_footstep_nodes */
01097     rg.remove_refzmp_cur_list_over_length(idx);
01098     /*   reset index and counter */
01099     rg.set_indices(idx);
01100     if (overwritable_footstep_index_offset == 0) {
01101         rg.set_refzmp_count(lcg.get_lcg_count()); // Start refzmp_count from current remaining footstep count of swinging.
01102     } else {
01103         rg.set_refzmp_count(static_cast<size_t>(fnsl[0][0].step_time/dt)); // Start refzmp_count from step length of first overwrite step
01104     }
01105     /*   reset refzmp */
01106     for (size_t i = 0; i < fnsl.size(); i++) {
01107         if (emergency_flg == EMERGENCY_STOP)
01108             rg.push_refzmp_from_footstep_nodes_for_dual(footstep_nodes_list[idx+i],
01109                                                         lcg.get_swing_leg_dst_steps_idx(footstep_nodes_list.size()-1),
01110                                                         lcg.get_support_leg_steps_idx(footstep_nodes_list.size()-1));
01111         else {
01112             if (i==fnsl.size()-1) {
01113                 rg.push_refzmp_from_footstep_nodes_for_dual(footstep_nodes_list[fnsl.size()-1],
01114                                                             lcg.get_swing_leg_dst_steps_idx(footstep_nodes_list.size()-1),
01115                                                             lcg.get_support_leg_steps_idx(footstep_nodes_list.size()-1));
01116             } else {
01117                 std::vector<step_node> tmp_swing_leg_src_steps;
01118                 lcg.calc_swing_leg_src_steps(tmp_swing_leg_src_steps, footstep_nodes_list, idx+i);
01119                 toe_heel_types tht(thtc.check_toe_heel_type_from_swing_support_coords(tmp_swing_leg_src_steps.front().worldcoords, lcg.get_support_leg_steps_idx(idx+i).front().worldcoords, lcg.get_toe_pos_offset_x(), lcg.get_heel_pos_offset_x()),
01120                                    thtc.check_toe_heel_type_from_swing_support_coords(lcg.get_swing_leg_dst_steps_idx(idx+i).front().worldcoords, lcg.get_support_leg_steps_idx(idx+i).front().worldcoords, lcg.get_toe_pos_offset_x(), lcg.get_heel_pos_offset_x()));
01121                 rg.push_refzmp_from_footstep_nodes_for_single(footstep_nodes_list[idx+i], lcg.get_support_leg_steps_idx(idx+i), tht);
01122             }
01123         }
01124     }
01125     /* Overwrite refzmp index in preview contoroller queue */
01126     size_t queue_size = preview_controller_ptr->get_preview_queue_size();
01127     size_t overwrite_idx;
01128     if (overwritable_footstep_index_offset == 0) {
01129       overwrite_idx = 0; // Overwrite all queue
01130     } else {
01131       overwrite_idx = lcg.get_lcg_count(); // Overwrite queue except current footstep
01132     }
01133     /* fill preview controller queue by new refzmp */
01134     hrp::Vector3 rzmp;
01135     bool refzmp_exist_p;
01136     std::vector<hrp::Vector3> sfzos;
01137     for (size_t i = overwrite_idx; i < queue_size - 1; i++) {
01138       refzmp_exist_p = rg.get_current_refzmp(rzmp, sfzos, default_double_support_ratio_before, default_double_support_ratio_after, default_double_support_static_ratio_before, default_double_support_static_ratio_after);
01139       preview_controller_ptr->set_preview_queue(rzmp, sfzos, i+1);
01140       rg.update_refzmp();
01141       sfzos.clear();
01142     }
01143     refzmp_exist_p = rg.get_current_refzmp(rzmp, sfzos, default_double_support_ratio_before, default_double_support_ratio_after, default_double_support_static_ratio_before, default_double_support_static_ratio_after);
01144     solved = preview_controller_ptr->update(refzmp, cog, swing_foot_zmp_offsets, rzmp, sfzos, refzmp_exist_p);
01145     rg.update_refzmp();
01146   };
01147 
01148   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 {
01149     std::vector<std::string> fns_names, cntr_legs_names;
01150     for (std::vector<step_node>::const_iterator it = fns.begin(); it != fns.end(); it++) {
01151         fns_names.push_back(leg_type_map.find(it->l_r)->second);
01152     }
01153     std::sort(_all_limbs.begin(), _all_limbs.end());
01154     std::sort(fns_names.begin(), fns_names.end());
01155     std::set_difference(_all_limbs.begin(), _all_limbs.end(), /* all candidates for legs */
01156                         fns_names.begin(), fns_names.end(),   /* support legs */
01157                         std::back_inserter(cntr_legs_names)); /* swing   legs */
01158     std::vector<leg_type> ret;
01159     for (std::vector<std::string>::const_iterator it = cntr_legs_names.begin(); it != cntr_legs_names.end(); it++) {
01160         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));
01161         ret.push_back(dst->first);
01162     }
01163     return ret;
01164   };
01165 }


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:54