ZMPDistributor.h
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #ifndef ZMP_DISTRIBUTOR_H
00011 #define ZMP_DISTRIBUTOR_H
00012 
00013 #include <hrpModel/Body.h>
00014 #include <iostream>
00015 #include <iterator>
00016 #include "../ImpedanceController/JointPathEx.h"
00017 #include "../TorqueFilter/IIRFilter.h"
00018 #include <hrpUtil/MatrixSolvers.h>
00019 
00020 #ifdef USE_QPOASES
00021 #include <qpOASES.hpp>
00022 using namespace qpOASES;
00023 #endif
00024 
00025 class FootSupportPolygon
00026 {
00027     std::vector<std::vector<Eigen::Vector2d> > foot_vertices; // RLEG, LLEG
00028 public:
00029     FootSupportPolygon () {};
00030     bool inside_foot (size_t idx)
00031     {
00032         return true;
00033     };
00034     Eigen::Vector2d get_foot_vertex (const size_t foot_idx, const size_t vtx_idx)
00035     {
00036         return foot_vertices[foot_idx][vtx_idx];
00037     };
00038     void set_vertices (const std::vector<std::vector<Eigen::Vector2d> >& vs) { foot_vertices = vs; };
00039     void get_vertices (std::vector<std::vector<Eigen::Vector2d> >& vs) { vs = foot_vertices; };
00040     void print_vertices (const std::string& str)
00041     {
00042         std::cerr << "[" << str << "]     ";
00043         for (size_t i = 0; i < foot_vertices.size(); i++) {
00044             std::cerr << "vs = ";
00045             for (size_t j = 0; j < foot_vertices[i].size(); j++) {
00046                 std::cerr << "[" << foot_vertices[i][j](0) << " " << foot_vertices[i][j](1) << "] ";
00047             }
00048             std::cerr << ((i==foot_vertices.size()-1)?"[m]":"[m], ");
00049         }
00050         std::cerr << std::endl;;
00051     }
00052 };
00053 
00054 //
00055 
00056 class SimpleZMPDistributor
00057 {
00058     FootSupportPolygon fs, fs_mgn;
00059     double leg_inside_margin, leg_outside_margin, leg_front_margin, leg_rear_margin, wrench_alpha_blending;
00060     boost::shared_ptr<FirstOrderLowPassFilter<double> > alpha_filter;
00061     std::vector<Eigen::Vector2d> convex_hull;
00062     enum projected_point_region {LEFT, MIDDLE, RIGHT};
00063 public:
00064     enum leg_type {RLEG, LLEG, RARM, LARM, BOTH, ALL};
00065     SimpleZMPDistributor (const double _dt) : wrench_alpha_blending (0.5)
00066     {
00067         alpha_filter = boost::shared_ptr<FirstOrderLowPassFilter<double> >(new FirstOrderLowPassFilter<double>(1e7, _dt, 0.5)); // [Hz], Almost no filter by default
00068     };
00069 
00070     inline bool is_inside_foot (const hrp::Vector3& leg_pos, const bool is_lleg, const double margin = 0.0)
00071     {
00072         if (is_lleg) return (leg_pos(1) >= (-1 * leg_inside_margin + margin)) && (leg_pos(1) <= (leg_outside_margin - margin));
00073         else return (leg_pos(1) <= (leg_inside_margin - margin)) && (leg_pos(1) >= (-1 * leg_outside_margin + margin));
00074     };
00075     inline bool is_front_of_foot (const hrp::Vector3& leg_pos, const double margin = 0.0)
00076     {
00077         return leg_pos(0) >= (leg_front_margin - margin);
00078     };
00079     inline bool is_rear_of_foot (const hrp::Vector3& leg_pos, const double margin = 0.0)
00080     {
00081         return leg_pos(0) <= (-1 * leg_rear_margin + margin);
00082     };
00083     inline bool is_inside_support_polygon (Eigen::Vector2d& p, const hrp::Vector3& offset = hrp::Vector3::Zero(), const bool& truncate_p = false, const std::string& str = "")
00084     {
00085       // set any inner point(ip) and binary search two vertices(convex_hull[v_a], convex_hull[v_b]) between which p is.
00086       p -= offset.head(2);
00087       size_t n_ch = convex_hull.size();
00088       Eigen::Vector2d ip = (convex_hull[0] + convex_hull[n_ch/3] + convex_hull[2*n_ch/3]) / 3.0;
00089       size_t v_a = 0, v_b = n_ch;
00090       while (v_a + 1 < v_b) {
00091         size_t v_c = (v_a + v_b) / 2;
00092         if (calcCrossProduct(convex_hull[v_a], convex_hull[v_c], ip) > 0) {
00093           if (calcCrossProduct(convex_hull[v_a], p, ip) > 0 && calcCrossProduct(convex_hull[v_c], p, ip) < 0) v_b = v_c;
00094           else v_a = v_c;
00095         } else {
00096           if (calcCrossProduct(convex_hull[v_a], p, ip) < 0 && calcCrossProduct(convex_hull[v_c], p, ip) > 0) v_a = v_c;
00097           else v_b = v_c;
00098         }
00099       }
00100       v_b %= n_ch;
00101       if (calcCrossProduct(convex_hull[v_a], convex_hull[v_b], p) >= 0) {
00102         p += offset.head(2);
00103         return true;
00104       } else {
00105         if (truncate_p) {
00106           if (!calc_closest_boundary_point(p, v_a, v_b)) std::cerr << "[" << str << "]   Cannot calculate closest boundary point on the convex hull" << std::endl;
00107         }
00108         p += offset.head(2);
00109         return false;
00110       }
00111     };
00112     void print_params (const std::string& str)
00113     {
00114         std::cerr << "[" << str << "]   leg_inside_margin = " << leg_inside_margin << "[m], leg_outside_margin = " << leg_outside_margin << "[m], leg_front_margin = " << leg_front_margin << "[m], leg_rear_margin = " << leg_rear_margin << "[m]" << std::endl;
00115         std::cerr << "[" << str << "]   wrench_alpha_blending = " << wrench_alpha_blending << ", alpha_cutoff_freq = " << alpha_filter->getCutOffFreq() << "[Hz]" << std::endl;
00116     }
00117     void print_vertices (const std::string& str)
00118     {
00119         fs.print_vertices(str);
00120     };
00121     // Compare Vector2d for sorting lexicographically
00122     static bool compare_eigen2d(const Eigen::Vector2d& lv, const Eigen::Vector2d& rv)
00123     {
00124       return lv(0) < rv(0) || (lv(0) == rv(0) && lv(1) < rv(1));
00125     };
00126     // Calculate 2D convex hull based on Andrew's algorithm
00127     // Assume that the order of vs, ee, and cs is the same
00128     void calc_convex_hull (const std::vector<std::vector<Eigen::Vector2d> >& vs, const std::vector<bool>& cs, const std::vector<hrp::Vector3>& ee_pos, const std::vector <hrp::Matrix33>& ee_rot)
00129     {
00130       // transform coordinate
00131       std::vector<Eigen::Vector2d>  tvs;
00132       hrp::Vector3 tpos;
00133       tvs.reserve(cs.size()*vs[0].size());
00134       for (size_t i = 0; i < cs.size(); i++) {
00135         if (cs[i]) {
00136           for (size_t j = 0; j < vs[i].size(); j++) {
00137             tpos = ee_pos[i] + ee_rot[i] * hrp::Vector3(vs[i][j](0), vs[i][j](1), 0.0);
00138             tvs.push_back(tpos.head(2));
00139           }
00140         }
00141       }
00142       // calculate convex hull
00143       int n_tvs = tvs.size(), n_ch = 0;
00144       convex_hull.resize(2*n_tvs);
00145       std::sort(tvs.begin(), tvs.end(), compare_eigen2d);
00146       for (int i = 0; i < n_tvs; convex_hull[n_ch++] = tvs[i++])
00147         while (n_ch >= 2 && calcCrossProduct(convex_hull[n_ch-1], tvs[i], convex_hull[n_ch-2]) <= 0) n_ch--;
00148       for (int i = n_tvs-2, j = n_ch+1; i >= 0; convex_hull[n_ch++] = tvs[i--])
00149         while (n_ch >= j && calcCrossProduct(convex_hull[n_ch-1], tvs[i], convex_hull[n_ch-2]) <= 0) n_ch--;
00150       convex_hull.resize(n_ch-1);
00151     };
00152     // Calculate closest boundary point on the convex hull
00153     bool calc_closest_boundary_point (Eigen::Vector2d& p, size_t& right_idx, size_t& left_idx) {
00154       size_t n_ch = convex_hull.size();
00155       Eigen::Vector2d cur_closest_point;
00156       for (size_t i; i < n_ch; i++) {
00157         switch(calcProjectedPoint(cur_closest_point, p, convex_hull[left_idx], convex_hull[right_idx])) {
00158         case MIDDLE:
00159           p = cur_closest_point;
00160           return true;
00161         case LEFT:
00162           right_idx = left_idx;
00163           left_idx = (left_idx + 1) % n_ch;
00164           if ((p - convex_hull[right_idx]).dot(convex_hull[left_idx] - convex_hull[right_idx]) <= 0) {
00165             p = cur_closest_point;
00166             return true;
00167           }
00168         case RIGHT:
00169           left_idx = right_idx;
00170           right_idx = (right_idx - 1) % n_ch;
00171           if ((p - convex_hull[left_idx]).dot(convex_hull[right_idx] - convex_hull[left_idx]) <= 0) {
00172             p = cur_closest_point;
00173             return true;
00174           }
00175         }
00176       }
00177       return false;
00178     }
00179     // setter
00180     void set_wrench_alpha_blending (const double a) { wrench_alpha_blending = a; };
00181     void set_leg_front_margin (const double a) { leg_front_margin = a; };
00182     void set_leg_rear_margin (const double a) { leg_rear_margin = a; };
00183     void set_leg_inside_margin (const double a) { leg_inside_margin = a; };
00184     void set_leg_outside_margin (const double a) { leg_outside_margin = a; };
00185     void set_alpha_cutoff_freq (const double a) { alpha_filter->setCutOffFreq(a); };
00186     void set_vertices (const std::vector<std::vector<Eigen::Vector2d> >& vs)
00187     {
00188         fs.set_vertices(vs);
00189         // leg_inside_margin = fs.get_foot_vertex(0, 0)(1);
00190         // leg_front_margin = fs.get_foot_vertex(0, 0)(0);
00191         // leg_rear_margin = std::fabs(fs.get_foot_vertex(0, 3)(0));
00192     };
00193     void set_vertices_from_margin_params ()
00194     {
00195         std::vector<std::vector<Eigen::Vector2d> > vec;
00196         // RLEG
00197         {
00198             std::vector<Eigen::Vector2d> tvec;
00199             tvec.push_back(Eigen::Vector2d(leg_front_margin, leg_inside_margin));
00200             tvec.push_back(Eigen::Vector2d(leg_front_margin, -1*leg_outside_margin));
00201             tvec.push_back(Eigen::Vector2d(-1*leg_rear_margin, -1*leg_outside_margin));
00202             tvec.push_back(Eigen::Vector2d(-1*leg_rear_margin, leg_inside_margin));
00203             vec.push_back(tvec);
00204         }
00205         // LLEG
00206         {
00207             std::vector<Eigen::Vector2d> tvec;
00208             tvec.push_back(Eigen::Vector2d(leg_front_margin, leg_outside_margin));
00209             tvec.push_back(Eigen::Vector2d(leg_front_margin, -1*leg_inside_margin));
00210             tvec.push_back(Eigen::Vector2d(-1*leg_rear_margin, -1*leg_inside_margin));
00211             tvec.push_back(Eigen::Vector2d(-1*leg_rear_margin, leg_outside_margin));
00212             vec.push_back(tvec);
00213         }
00214         // {
00215         //     std::vector<Eigen::Vector2d> tvec;
00216         //     tvec.push_back(Eigen::Vector2d(leg_front_margin, leg_inside_margin));
00217         //     tvec.push_back(Eigen::Vector2d(leg_front_margin, -1*leg_outside_margin));
00218         //     tvec.push_back(Eigen::Vector2d(-1*leg_rear_margin, -1*leg_outside_margin));
00219         //     tvec.push_back(Eigen::Vector2d(-1*leg_rear_margin, leg_inside_margin));
00220         //     vec.push_back(tvec);
00221         // }
00222         // {
00223         //     std::vector<Eigen::Vector2d> tvec;
00224         //     tvec.push_back(Eigen::Vector2d(leg_front_margin, leg_inside_margin));
00225         //     tvec.push_back(Eigen::Vector2d(leg_front_margin, -1*leg_outside_margin));
00226         //     tvec.push_back(Eigen::Vector2d(-1*leg_rear_margin, -1*leg_outside_margin));
00227         //     tvec.push_back(Eigen::Vector2d(-1*leg_rear_margin, leg_inside_margin));
00228         //     vec.push_back(tvec);
00229         // }
00230         set_vertices(vec);
00231     };
00232     // Set vertices only for cp_check_margin for now
00233     void set_vertices_from_margin_params (const std::vector<double>& margin)
00234     {
00235       std::vector<std::vector<Eigen::Vector2d> > vec;
00236       // RLEG
00237       {
00238         std::vector<Eigen::Vector2d> tvec;
00239         tvec.push_back(Eigen::Vector2d(leg_front_margin - margin[0], leg_inside_margin - margin[2]));
00240         tvec.push_back(Eigen::Vector2d(leg_front_margin - margin[0], -1*(leg_outside_margin - margin[3])));
00241         tvec.push_back(Eigen::Vector2d(-1*(leg_rear_margin - margin[1]), -1*(leg_outside_margin - margin[3])));
00242         tvec.push_back(Eigen::Vector2d(-1*(leg_rear_margin - margin[1]), leg_inside_margin - margin[2]));
00243         vec.push_back(tvec);
00244       }
00245       // LLEG
00246       {
00247         std::vector<Eigen::Vector2d> tvec;
00248         tvec.push_back(Eigen::Vector2d(leg_front_margin - margin[0], leg_inside_margin - margin[3]));
00249         tvec.push_back(Eigen::Vector2d(leg_front_margin - margin[0], -1*(leg_outside_margin - margin[2])));
00250         tvec.push_back(Eigen::Vector2d(-1*(leg_rear_margin - margin[1]), -1*(leg_outside_margin - margin[2])));
00251         tvec.push_back(Eigen::Vector2d(-1*(leg_rear_margin - margin[1]), leg_inside_margin - margin[3]));
00252         vec.push_back(tvec);
00253       }
00254       fs_mgn.set_vertices(vec);
00255     };
00256     // getter
00257     double get_wrench_alpha_blending () { return wrench_alpha_blending; };
00258     double get_leg_front_margin () { return leg_front_margin; };
00259     double get_leg_rear_margin () { return leg_rear_margin; };
00260     double get_leg_inside_margin () { return leg_inside_margin; };
00261     double get_leg_outside_margin () { return leg_outside_margin; };
00262     double get_alpha_cutoff_freq () { return alpha_filter->getCutOffFreq(); };
00263     void get_vertices (std::vector<std::vector<Eigen::Vector2d> >& vs) { fs.get_vertices(vs); };
00264     void get_margined_vertices (std::vector<std::vector<Eigen::Vector2d> >& vs) { fs_mgn.get_vertices(vs); };
00265     //
00266     double calcAlpha (const hrp::Vector3& tmprefzmp,
00267                       const std::vector<hrp::Vector3>& ee_pos,
00268                       const std::vector<hrp::Matrix33>& ee_rot,
00269                       const std::vector<std::string>& ee_name)
00270     {
00271         double alpha;
00272         size_t l_idx, r_idx;
00273         for (size_t i = 0; i < ee_name.size(); i++) {
00274             if (ee_name[i]=="rleg") r_idx = i;
00275             else l_idx = i;
00276         }
00277         hrp::Vector3 l_local_zmp = ee_rot[l_idx].transpose() * (tmprefzmp-ee_pos[l_idx]);
00278         hrp::Vector3 r_local_zmp = ee_rot[r_idx].transpose() * (tmprefzmp-ee_pos[r_idx]);
00279 
00280         // std::cerr << "a " << l_local_zmp(0) << " " << l_local_zmp(1) << std::endl;
00281         // std::cerr << "b " << r_local_zmp(0) << " " << r_local_zmp(1) << std::endl;
00282         if ( is_inside_foot(l_local_zmp, true) && !is_front_of_foot(l_local_zmp) && !is_rear_of_foot(l_local_zmp)) { // new_refzmp is inside lfoot
00283             alpha = 0.0;
00284         } else if ( is_inside_foot(r_local_zmp, false) && !is_front_of_foot(r_local_zmp) && !is_rear_of_foot(r_local_zmp)) { // new_refzmp is inside rfoot
00285             alpha = 1.0;
00286         } else {
00287             hrp::Vector3 ledge_foot;
00288             hrp::Vector3 redge_foot;
00289             // lleg
00290             if (is_inside_foot(l_local_zmp, true) && is_front_of_foot(l_local_zmp)) {
00291                 ledge_foot = hrp::Vector3(leg_front_margin, l_local_zmp(1), 0.0);
00292             } else if (!is_inside_foot(l_local_zmp, true) && is_front_of_foot(l_local_zmp)) {
00293                 ledge_foot = hrp::Vector3(leg_front_margin, -1 * leg_inside_margin, 0.0);
00294             } else if (!is_inside_foot(l_local_zmp, true) && !is_front_of_foot(l_local_zmp) && !is_rear_of_foot(l_local_zmp)) {
00295                 ledge_foot = hrp::Vector3(l_local_zmp(0), -1 * leg_inside_margin, 0.0);
00296             } else if (!is_inside_foot(l_local_zmp, true) && is_rear_of_foot(l_local_zmp)) {
00297                 ledge_foot = hrp::Vector3(-1 * leg_rear_margin, -1 * leg_inside_margin, 0.0);
00298             } else {
00299                 ledge_foot = hrp::Vector3(-1 * leg_rear_margin, l_local_zmp(1), 0.0);
00300             }
00301             ledge_foot = ee_rot[l_idx] * ledge_foot + ee_pos[l_idx];
00302             // rleg
00303             if (is_inside_foot(r_local_zmp, false) && is_front_of_foot(r_local_zmp)) {
00304                 redge_foot = hrp::Vector3(leg_front_margin, r_local_zmp(1), 0.0);
00305             } else if (!is_inside_foot(r_local_zmp, false) && is_front_of_foot(r_local_zmp)) {
00306                 redge_foot = hrp::Vector3(leg_front_margin, leg_inside_margin, 0.0);
00307             } else if (!is_inside_foot(r_local_zmp, false) && !is_front_of_foot(r_local_zmp) && !is_rear_of_foot(r_local_zmp)) {
00308                 redge_foot = hrp::Vector3(r_local_zmp(0), leg_inside_margin, 0.0);
00309             } else if (!is_inside_foot(r_local_zmp, false) && is_rear_of_foot(r_local_zmp)) {
00310                 redge_foot = hrp::Vector3(-1 * leg_rear_margin, leg_inside_margin, 0.0);
00311             } else {
00312                 redge_foot = hrp::Vector3(-1 * leg_rear_margin, r_local_zmp(1), 0.0);
00313             }
00314             redge_foot = ee_rot[r_idx] * redge_foot + ee_pos[r_idx];
00315             // calc alpha
00316             hrp::Vector3 difp = redge_foot - ledge_foot;
00317             alpha = difp.dot(tmprefzmp-ledge_foot)/difp.squaredNorm();
00318         }
00319         // limit
00320         if (alpha>1.0) alpha = 1.0;
00321         if (alpha<0.0) alpha = 0.0;
00322         return alpha;
00323     };
00324 
00325     double calcAlphaFromCOP (const hrp::Vector3& tmprefzmp,
00326                              const std::vector<hrp::Vector3>& cop_pos,
00327                              const std::vector<std::string>& ee_name)
00328     {
00329         size_t l_idx, r_idx;
00330         for (size_t i = 0; i < ee_name.size(); i++) {
00331             if (ee_name[i]=="rleg") r_idx = i;
00332             else l_idx = i;
00333         }
00334         hrp::Vector3 difp = (cop_pos[r_idx]-cop_pos[l_idx]);
00335         double alpha = difp.dot(tmprefzmp - cop_pos[l_idx])/difp.dot(difp);
00336 
00337         // limit
00338         if (alpha>1.0) alpha = 1.0;
00339         if (alpha<0.0) alpha = 0.0;
00340         return alpha;
00341     };
00342 
00343     void calcAlphaVector (std::vector<double>& alpha_vector,
00344                           std::vector<double>& fz_alpha_vector,
00345                           const std::vector<hrp::Vector3>& ee_pos,
00346                           const std::vector<hrp::Matrix33>& ee_rot,
00347                           const std::vector<std::string>& ee_name,
00348                           const hrp::Vector3& new_refzmp, const hrp::Vector3& ref_zmp)
00349     {
00350         double fz_alpha =  calcAlpha(ref_zmp, ee_pos, ee_rot, ee_name);
00351         double tmpalpha = calcAlpha(new_refzmp, ee_pos, ee_rot, ee_name), alpha;
00352         // LPF
00353         alpha = alpha_filter->passFilter(tmpalpha);
00354         // Blending
00355         fz_alpha = wrench_alpha_blending * fz_alpha + (1-wrench_alpha_blending) * alpha;
00356         for (size_t i = 0; i < ee_name.size(); i++) {
00357             alpha_vector[i]= (ee_name[i]=="rleg") ? alpha : 1-alpha;
00358             fz_alpha_vector[i]=(ee_name[i]=="rleg") ? fz_alpha : 1-fz_alpha;
00359         }
00360     };
00361 
00362     void calcAlphaVectorFromCOP (std::vector<double>& alpha_vector,
00363                                  std::vector<double>& fz_alpha_vector,
00364                                  const std::vector<hrp::Vector3>& cop_pos,
00365                                  const std::vector<std::string>& ee_name,
00366                                  const hrp::Vector3& new_refzmp, const hrp::Vector3& ref_zmp)
00367     {
00368         double fz_alpha =  calcAlphaFromCOP(ref_zmp, cop_pos, ee_name);
00369         double tmpalpha = calcAlphaFromCOP(new_refzmp, cop_pos, ee_name), alpha;
00370         // LPF
00371         alpha = alpha_filter->passFilter(tmpalpha);
00372         // Blending
00373         fz_alpha = wrench_alpha_blending * fz_alpha + (1-wrench_alpha_blending) * alpha;
00374         for (size_t i = 0; i < ee_name.size(); i++) {
00375             alpha_vector[i]= (ee_name[i]=="rleg") ? alpha : 1-alpha;
00376             fz_alpha_vector[i]=(ee_name[i]=="rleg") ? fz_alpha : 1-fz_alpha;
00377         }
00378     };
00379 
00380     void calcAlphaVectorFromCOPDistanceCommon (std::vector<double>& alpha_vector,
00381                                                const std::vector<hrp::Vector3>& cop_pos,
00382                                                const std::vector<std::string>& ee_name,
00383                                                const hrp::Vector3& ref_zmp)
00384     {
00385         std::vector<double> distances;
00386         double tmpdistance = 0;
00387         for (size_t i = 0; i < ee_name.size(); i++) {
00388             distances.push_back((cop_pos[i]-ref_zmp).norm());
00389             tmpdistance += (cop_pos[i]-ref_zmp).norm();
00390         }
00391         for (size_t i = 0; i < ee_name.size(); i++) {
00392             alpha_vector[i] = tmpdistance/distances[i];
00393         }
00394     };
00395 
00396     void calcAlphaVectorFromCOPDistance (std::vector<double>& alpha_vector,
00397                                          std::vector<double>& fz_alpha_vector,
00398                                          const std::vector<hrp::Vector3>& cop_pos,
00399                                          const std::vector<std::string>& ee_name,
00400                                          const hrp::Vector3& new_refzmp, const hrp::Vector3& ref_zmp)
00401     {
00402         calcAlphaVectorFromCOPDistanceCommon(alpha_vector, cop_pos, ee_name, new_refzmp);
00403         calcAlphaVectorFromCOPDistanceCommon(fz_alpha_vector, cop_pos, ee_name, ref_zmp);
00404         for (size_t i = 0; i < ee_name.size(); i++) {
00405             fz_alpha_vector[i] = wrench_alpha_blending * fz_alpha_vector[i] + (1-wrench_alpha_blending) * alpha_vector[i];
00406         }
00407     };
00408 
00409     void distributeZMPToForceMoments (std::vector<hrp::Vector3>& ref_foot_force, std::vector<hrp::Vector3>& ref_foot_moment,
00410                                       const std::vector<hrp::Vector3>& ee_pos,
00411                                       const std::vector<hrp::Vector3>& cop_pos,
00412                                       const std::vector<hrp::Matrix33>& ee_rot,
00413                                       const std::vector<std::string>& ee_name,
00414                                       const std::vector<double>& limb_gains,
00415                                       const std::vector<double>& toeheel_ratio,
00416                                       const hrp::Vector3& new_refzmp, const hrp::Vector3& ref_zmp,
00417                                       const double total_fz, const double dt, const bool printp = true, const std::string& print_str = "")
00418     {
00419         std::vector<double> alpha_vector(2), fz_alpha_vector(2);
00420         calcAlphaVector(alpha_vector, fz_alpha_vector, ee_pos, ee_rot, ee_name, new_refzmp, ref_zmp);
00421         ref_foot_force[0] = hrp::Vector3(0,0, fz_alpha_vector[0] * total_fz);
00422         ref_foot_force[1] = hrp::Vector3(0,0, fz_alpha_vector[1] * total_fz);
00423 
00424         hrp::Vector3 tau_0 = hrp::Vector3::Zero();
00425 #if 0
00426         double gamma = fz_alpha;
00427         double beta = m_wrenches[1].data[2] / ( m_wrenches[0].data[2] + m_wrenches[1].data[2] );
00428         beta = isnan(beta) ? 0 : beta;
00429         double steepness = 8; // change ration from alpha to beta (steepness >= 4)
00430         double r = - 1/(1+exp(-6*steepness*(gamma-1+1/steepness))) + 1/(1+exp(-6*steepness*(gamma-1/steepness)));
00431         fz_alpha = r * beta + ( 1 - r ) * gamma;
00432         //       alpha = fz_alpha;
00433 
00434         ref_foot_force[0] = hrp::Vector3(0,0, fz_alpha * total_fz);
00435         ref_foot_force[1] = hrp::Vector3(0,0, (1-fz_alpha) * total_fz);
00436         if (DEBUGP) {
00437             std::cerr << "[" << m_profile.instance_name << "] slip st parameters" << std::endl;
00438             std::cerr << "[" << m_profile.instance_name << "]   " << ref_foot_force[1](2) << " " << ref_foot_force[0](2) << "   a:"<< steepness << " beta:" << beta << " gamma:" << gamma << " r:" << r << " fz_alpha:" << fz_alpha <<  " alpha:" << alpha << std::endl;
00439         }
00440 #endif
00441 
00442         for (size_t i = 0; i < 2; i++) {
00443             tau_0 -= (cop_pos[i] - new_refzmp).cross(ref_foot_force[i]);
00444         }
00445         {
00446             // Foot-distribution-coords frame =>
00447 //             hrp::Vector3 foot_dist_coords_y = (cop_pos[1] - cop_pos[0]); // e_y'
00448 //             foot_dist_coords_y(2) = 0.0;
00449 //             foot_dist_coords_y.normalize();
00450 //             hrp::Vector3 foot_dist_coords_x = hrp::Vector3(foot_dist_coords_y.cross(hrp::Vector3::UnitZ())); // e_x'
00451 //             hrp::Matrix33 foot_dist_coords_rot;
00452 //             foot_dist_coords_rot(0,0) = foot_dist_coords_x(0);
00453 //             foot_dist_coords_rot(1,0) = foot_dist_coords_x(1);
00454 //             foot_dist_coords_rot(2,0) = foot_dist_coords_x(2);
00455 //             foot_dist_coords_rot(0,1) = foot_dist_coords_y(0);
00456 //             foot_dist_coords_rot(1,1) = foot_dist_coords_y(1);
00457 //             foot_dist_coords_rot(2,1) = foot_dist_coords_y(2);
00458 //             foot_dist_coords_rot(0,2) = 0;
00459 //             foot_dist_coords_rot(1,2) = 0;
00460 //             foot_dist_coords_rot(2,2) = 1;
00461 //             hrp::Vector3 tau_0_f = foot_dist_coords_rot.transpose() * tau_0; // tau_0'
00462 //             // x
00463 //             //         // right
00464 //             //         if (tau_0_f(0) > 0) ref_foot_moment[0](0) = tau_0_f(0);
00465 //             //         else ref_foot_moment[0](0) = 0;
00466 //             //         // left
00467 //             //         if (tau_0_f(0) > 0) ref_foot_moment[1](0) = 0;
00468 //             //         else ref_foot_moment[1](0) = tau_0_f(0);
00469 //             ref_foot_moment[0](0) = tau_0_f(0) * alpha;
00470 //             ref_foot_moment[1](0) = tau_0_f(0) * (1-alpha);
00471 //             // y
00472 //             ref_foot_moment[0](1) = tau_0_f(1) * alpha;
00473 //             ref_foot_moment[1](1) = tau_0_f(1) * (1-alpha);
00474 //             ref_foot_moment[0](2) = ref_foot_moment[1](2) = 0.0;
00475 //             // <= Foot-distribution-coords frame
00476 //             // Convert foot-distribution-coords frame => actual world frame
00477 //             ref_foot_moment[0] = foot_dist_coords_rot * ref_foot_moment[0];
00478 //             ref_foot_moment[1] = foot_dist_coords_rot * ref_foot_moment[1];
00479             //
00480           ref_foot_moment[0](0) = tau_0(0) * alpha_vector[0];
00481           ref_foot_moment[1](0) = tau_0(0) * alpha_vector[1];
00482           ref_foot_moment[0](1) = tau_0(1) * alpha_vector[0];
00483           ref_foot_moment[1](1) = tau_0(1) * alpha_vector[1];
00484           ref_foot_moment[0](2) = ref_foot_moment[1](2)= 0.0;
00485         }
00486 #if 0
00487         {
00488             // Foot-distribution-coords frame =>
00489             hrp::Vector3 foot_dist_coords_y = (cop_pos[1] - cop_pos[0]); // e_y'
00490             foot_dist_coords_y(2) = 0.0;
00491             foot_dist_coords_y.normalize();
00492             hrp::Vector3 foot_dist_coords_x = hrp::Vector3(foot_dist_coords_y.cross(hrp::Vector3::UnitZ())); // e_x'
00493             hrp::Matrix33 foot_dist_coords_rot;
00494             foot_dist_coords_rot(0,0) = foot_dist_coords_x(0);
00495             foot_dist_coords_rot(1,0) = foot_dist_coords_x(1);
00496             foot_dist_coords_rot(2,0) = foot_dist_coords_x(2);
00497             foot_dist_coords_rot(0,1) = foot_dist_coords_y(0);
00498             foot_dist_coords_rot(1,1) = foot_dist_coords_y(1);
00499             foot_dist_coords_rot(2,1) = foot_dist_coords_y(2);
00500             foot_dist_coords_rot(0,2) = 0;
00501             foot_dist_coords_rot(1,2) = 0;
00502             foot_dist_coords_rot(2,2) = 1;
00503             tau_0 = hrp::Vector3::Zero();
00504             //
00505             hrp::dvector fvec(3);
00506             fvec(0) = total_fz;
00507             fvec(1) = tau_0(0);
00508             fvec(2) = tau_0(1);
00509             hrp::dmatrix Gmat(3,6);
00510             Gmat(0,0) = 1.0; Gmat(0,1) = 0.0; Gmat(0,2) = 0.0;
00511             Gmat(0,3) = 1.0; Gmat(0,4) = 0.0; Gmat(0,5) = 0.0;
00512             Gmat(1,0) = (cop_pos[0](1)-new_refzmp(1));
00513             Gmat(1,1) = 1.0;
00514             Gmat(1,2) = 0.0;
00515             Gmat(1,3) = (cop_pos[1](1)-new_refzmp(1));
00516             Gmat(1,4) = 1.0;
00517             Gmat(1,5) = 0.0;
00518             Gmat(2,0) = -(cop_pos[0](0)-new_refzmp(0));
00519             Gmat(2,1) = 0.0;
00520             Gmat(2,2) = 1.0;
00521             Gmat(2,3) = -(cop_pos[1](0)-new_refzmp(0));
00522             Gmat(2,4) = 0.0;
00523             Gmat(2,5) = 1.0;
00524             hrp::dmatrix Wmat(6,6);
00525             for (size_t i = 0; i < 6; i++) {
00526                 for (size_t j = 0; j < 6; j++) {
00527                     Wmat(i,j) = 0.0;
00528                 }
00529             }
00530             double beta_r =0 , beta_l =0;
00531             double kk = 8.0;
00532             double alpha_r = 0.9;
00533             double alpha_l = 0.1;
00534             if (alpha > alpha_r) beta_r = std::pow((alpha/alpha_r-1.0), kk)/std::pow((1.0/alpha_r-1.0), kk);
00535             else beta_r = 0;
00536             if (alpha < alpha_l) beta_l = std::pow((alpha/alpha_l-1.0), kk);
00537             else beta_l = 0;
00538             Wmat(0,0) = alpha;
00539             Wmat(1,1) = beta_r;
00540             Wmat(2,2) = alpha;
00541             Wmat(3,3) = (1-alpha);
00542             Wmat(5,5) = (1-alpha);
00543             Wmat(4,4) = beta_l;
00544             // if (printp) {
00545             //     std::cerr << Wmat << std::endl;
00546             // }
00547             hrp::dmatrix Gmat_ret(6,3);
00548             hrp::calcSRInverse(Gmat, Gmat_ret, 0.0, Wmat);
00549             hrp::dvector fmvec(6);
00550             fmvec = Gmat_ret* fvec;
00551             ref_foot_force[0] = hrp::Vector3(0,0,fmvec(0));
00552             ref_foot_force[1] = hrp::Vector3(0,0,fmvec(3));
00553             ref_foot_moment[0] = hrp::Vector3(fmvec(1),fmvec(2),0);
00554             ref_foot_moment[1] = hrp::Vector3(fmvec(4),fmvec(5),0);
00555             // <= Foot-distribution-coords frame
00556             // Convert foot-distribution-coords frame => actual world frame
00557             ref_foot_moment[0] = foot_dist_coords_rot * ref_foot_moment[0];
00558             ref_foot_moment[1] = foot_dist_coords_rot * ref_foot_moment[1];
00559         }
00560 #endif
00561 
00562         if (printp) {
00563             //std::cerr << "[" << print_str << "]   alpha = " << alpha << ", fz_alpha = " << fz_alpha << std::endl;
00564             std::cerr << "[" << print_str << "]   "
00565                       << "total_tau    = " << hrp::Vector3(tau_0).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
00566             std::cerr << "[" << print_str << "]   "
00567                       << "ref_force_R  = " << hrp::Vector3(ref_foot_force[0]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N]" << std::endl;
00568             std::cerr << "[" << print_str << "]   "
00569                       << "ref_force_L  = " << hrp::Vector3(ref_foot_force[1]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N]" << std::endl;
00570             std::cerr << "[" << print_str << "]   "
00571                       << "ref_moment_R = " << hrp::Vector3(ref_foot_moment[0]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
00572             std::cerr << "[" << print_str << "]   "
00573                       << "ref_moment_L = " << hrp::Vector3(ref_foot_moment[1]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
00574         }
00575     };
00576 
00577 #ifdef USE_QPOASES
00578     void solveForceMomentQPOASES (std::vector<hrp::dvector>& fret,
00579                                   const size_t state_dim,
00580                                   const size_t ee_num,
00581                                   const hrp::dmatrix& Hmat,
00582                                   const hrp::dvector& gvec)
00583     {
00584         real_t* H = new real_t[state_dim*state_dim];
00585         real_t* g = new real_t[state_dim];
00586         real_t* lb = new real_t[state_dim];
00587         real_t* ub = new real_t[state_dim];
00588         for (size_t i = 0; i < state_dim; i++) {
00589             for (size_t j = 0; j < state_dim; j++) {
00590                 H[i*state_dim+j] = Hmat(i,j);
00591             }
00592             g[i] = gvec(i);
00593             lb[i] = 0.0;
00594             ub[i] = 1e10;
00595         }
00596         QProblemB example( state_dim );
00597         Options options;
00598         //options.enableFlippingBounds = BT_FALSE;
00599         options.initialStatusBounds = ST_INACTIVE;
00600         options.numRefinementSteps = 1;
00601         options.enableCholeskyRefactorisation = 1;
00602         //options.printLevel = PL_LOW;
00603         options.printLevel = PL_NONE;
00604         example.setOptions( options );
00605         /* Solve first QP. */
00606         int nWSR = 10;
00607         example.init( H,g,lb,ub, nWSR,0 );
00608         real_t* xOpt = new real_t[state_dim];
00609         example.getPrimalSolution( xOpt );
00610         size_t state_dim_one = state_dim / ee_num;
00611         for (size_t fidx = 0; fidx < ee_num; fidx++) {
00612             for (size_t i = 0; i < state_dim_one; i++) {
00613                 fret[fidx](i) = xOpt[i+state_dim_one*fidx];
00614             }
00615         }
00616         delete[] H;
00617         delete[] g;
00618         delete[] lb;
00619         delete[] ub;
00620         delete[] xOpt;
00621     };
00622 
00623     void distributeZMPToForceMomentsQP (std::vector<hrp::Vector3>& ref_foot_force, std::vector<hrp::Vector3>& ref_foot_moment,
00624                                         const std::vector<hrp::Vector3>& ee_pos,
00625                                         const std::vector<hrp::Vector3>& cop_pos,
00626                                         const std::vector<hrp::Matrix33>& ee_rot,
00627                                         const std::vector<std::string>& ee_name,
00628                                         const std::vector<double>& limb_gains,
00629                                         const std::vector<double>& toeheel_ratio,
00630                                         const hrp::Vector3& new_refzmp, const hrp::Vector3& ref_zmp,
00631                                         const double total_fz, const double dt, const bool printp = true, const std::string& print_str = "",
00632                                         const bool use_cop_distribution = false)
00633     {
00634         size_t ee_num = ee_name.size();
00635         std::vector<double> alpha_vector(ee_num), fz_alpha_vector(ee_num);
00636         if ( use_cop_distribution ) {
00637             //calcAlphaVectorFromCOP(alpha_vector, fz_alpha_vector, cop_pos, ee_name, new_refzmp, ref_zmp);
00638             calcAlphaVectorFromCOPDistance(alpha_vector, fz_alpha_vector, cop_pos, ee_name, new_refzmp, ref_zmp);
00639         } else {
00640             calcAlphaVector(alpha_vector, fz_alpha_vector, ee_pos, ee_rot, ee_name, new_refzmp, ref_zmp);
00641         }
00642 
00643         // QP
00644         double norm_weight = 1e-7;
00645         double cop_weight = 1e-3;
00646         double ref_force_weight = 0;// 1e-3;
00647         hrp::dvector total_fm(3);
00648         total_fm(0) = total_fz;
00649         total_fm(1) = 0;
00650         total_fm(2) = 0;
00651         size_t state_dim = 4*ee_num, state_dim_one = 4; // TODO
00652         //
00653         std::vector<hrp::dvector> ff(ee_num, hrp::dvector(state_dim_one));
00654         std::vector<hrp::dmatrix> mm(ee_num, hrp::dmatrix(3, state_dim_one));
00655         //
00656         hrp::dmatrix Hmat = hrp::dmatrix::Zero(state_dim,state_dim);
00657         hrp::dvector gvec = hrp::dvector::Zero(state_dim);
00658         double alpha_thre = 1e-20;
00659         // fz_alpha inversion for weighing matrix
00660         for (size_t i = 0; i < fz_alpha_vector.size(); i++) {
00661             fz_alpha_vector[i] *= limb_gains[i];
00662             fz_alpha_vector[i] = (fz_alpha_vector[i] < alpha_thre) ? 1/alpha_thre : 1/fz_alpha_vector[i];
00663         }
00664         for (size_t j = 0; j < fz_alpha_vector.size(); j++) {
00665             for (size_t i = 0; i < state_dim_one; i++) {
00666                 Hmat(i+j*state_dim_one,i+j*state_dim_one) = norm_weight * fz_alpha_vector[j];
00667             }
00668         }
00669         hrp::dmatrix Gmat(3,state_dim);
00670         for (size_t i = 0; i < state_dim; i++) {
00671             Gmat(0,i) = 1.0;
00672         }
00673         for (size_t fidx = 0; fidx < ee_num; fidx++) {
00674             for (size_t i = 0; i < state_dim_one; i++) {
00675                 hrp::Vector3 fpos = ee_rot[fidx]*hrp::Vector3(fs.get_foot_vertex(fidx,i)(0), fs.get_foot_vertex(fidx,i)(1), 0) + ee_pos[fidx];
00676                 mm[fidx](0,i) = 1.0;
00677                 mm[fidx](1,i) = -(fpos(1)-cop_pos[fidx](1));
00678                 mm[fidx](2,i) = (fpos(0)-cop_pos[fidx](0));
00679                 Gmat(1,i+state_dim_one*fidx) = -(fpos(1)-new_refzmp(1));
00680                 Gmat(2,i+state_dim_one*fidx) = (fpos(0)-new_refzmp(0));
00681             }
00682             //std::cerr << "fpos " << fpos[0] << " " << fpos[1] << std::endl;
00683         }
00684         Hmat += Gmat.transpose() * Gmat;
00685         gvec += -1 * Gmat.transpose() * total_fm;
00686         // std::cerr << "Gmat " << std::endl;
00687         // std::cerr << Gmat << std::endl;
00688         // std::cerr << "total_fm " << std::endl;
00689         // std::cerr << total_fm << std::endl;
00690         //
00691         {
00692             hrp::dmatrix Kmat = hrp::dmatrix::Zero(ee_num,state_dim);
00693             hrp::dmatrix KW = hrp::dmatrix::Zero(ee_num, ee_num);
00694             hrp::dvector reff(ee_num);
00695             for (size_t j = 0; j < ee_num; j++) {
00696                 for (size_t i = 0; i < state_dim_one; i++) {
00697                     Kmat(j,i+j*state_dim_one) = 1.0;
00698                 }
00699                 reff(j) = ref_foot_force[j](2);// total_fz/2.0;
00700                 KW(j,j) = ref_force_weight;
00701             }
00702             Hmat += Kmat.transpose() * KW * Kmat;
00703             gvec += -1 * Kmat.transpose() * KW * reff;
00704         }
00705         {
00706             hrp::dmatrix Cmat = hrp::dmatrix::Zero(ee_num*2,state_dim);
00707             hrp::dmatrix CW = hrp::dmatrix::Zero(ee_num*2,ee_num*2);
00708             hrp::Vector3 fpos;
00709             for (size_t j = 0; j < ee_num; j++) {
00710                 for (size_t i = 0; i < state_dim_one; i++) {
00711                     fpos = ee_rot[j]*hrp::Vector3(fs.get_foot_vertex(j,i)(0), fs.get_foot_vertex(j,i)(1), 0) + ee_pos[j];
00712                     Cmat(j*2,  i+j*state_dim_one) = fpos(0) - cop_pos[j](0);
00713                     Cmat(j*2+1,i+j*state_dim_one) = fpos(1) - cop_pos[j](1);
00714                 }
00715                 CW(j*2,j*2) = CW(j*2+1,j*2+1) = cop_weight;
00716             }
00717             Hmat += Cmat.transpose() * CW * Cmat;
00718         }
00719         // std::cerr << "H " << Hmat << std::endl;
00720         // std::cerr << "g " << gvec << std::endl;
00721         solveForceMomentQPOASES(ff, state_dim, ee_num, Hmat, gvec);
00722         hrp::dvector tmpv(3);
00723         for (size_t fidx = 0; fidx < ee_num; fidx++) {
00724             tmpv = mm[fidx] * ff[fidx];
00725             ref_foot_force[fidx] = hrp::Vector3(0,0,tmpv(0));
00726             ref_foot_moment[fidx] = -1*hrp::Vector3(tmpv(1),tmpv(2),0);
00727         }
00728         if (printp) {
00729             std::cerr << "[" << print_str << "] force moment distribution " << (use_cop_distribution ? "(QP COP)" : "(QP)") << std::endl;
00730             //std::cerr << "[" << print_str << "]   alpha = " << alpha << ", fz_alpha = " << fz_alpha << std::endl;
00731             // std::cerr << "[" << print_str << "]   "
00732             //           << "total_tau    = " << hrp::Vector3(tau_0).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
00733             for (size_t i = 0; i < ee_num; i++) {
00734                 std::cerr << "[" << print_str << "]   "
00735                           << "ref_force  [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_force[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N]" << std::endl;
00736                 std::cerr << "[" << print_str << "]   "
00737                           << "ref_moment [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_moment[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
00738             }
00739         }
00740     };
00741 #else
00742     void distributeZMPToForceMomentsQP (std::vector<hrp::Vector3>& ref_foot_force, std::vector<hrp::Vector3>& ref_foot_moment,
00743                                         const std::vector<hrp::Vector3>& ee_pos,
00744                                         const std::vector<hrp::Vector3>& cop_pos,
00745                                         const std::vector<hrp::Matrix33>& ee_rot,
00746                                         const std::vector<std::string>& ee_name,
00747                                         const std::vector<double>& limb_gains,
00748                                         const std::vector<double>& toeheel_ratio,
00749                                         const hrp::Vector3& new_refzmp, const hrp::Vector3& ref_zmp,
00750                                         const double total_fz, const double dt, const bool printp = true, const std::string& print_str = "",
00751                                         const bool use_cop_distribution = false)
00752     {
00753         distributeZMPToForceMoments(ref_foot_force, ref_foot_moment,
00754                                     ee_pos, cop_pos, ee_rot, ee_name, limb_gains, toeheel_ratio,
00755                                     new_refzmp, ref_zmp,
00756                                     total_fz, dt, printp, print_str);
00757     };
00758 #endif // USE_QPOASES
00759 
00760     // Solve A * x = b => x = W A^T (A W A^T)-1 b
00761     // => x = W^{1/2} Pinv(A W^{1/2}) b
00762     void calcWeightedLinearEquation(hrp::dvector& ret, const hrp::dmatrix& A, const hrp::dmatrix& W, const hrp::dvector& b)
00763     {
00764         hrp::dmatrix W2 = hrp::dmatrix::Zero(W.rows(), W.cols());
00765         for (size_t i = 0; i < W.rows(); i++) W2(i,i) = std::sqrt(W(i,i));
00766         hrp::dmatrix Aw = A*W2;
00767         hrp::dmatrix Aw_inv = hrp::dmatrix::Zero(A.cols(), A.rows());
00768         hrp::calcPseudoInverse(Aw, Aw_inv);
00769         ret = W2 * Aw_inv * b;
00770         //ret = W2 * Aw.colPivHouseholderQr().solve(b);
00771     };
00772 
00773     void distributeZMPToForceMomentsPseudoInverse (std::vector<hrp::Vector3>& ref_foot_force, std::vector<hrp::Vector3>& ref_foot_moment,
00774                                                    const std::vector<hrp::Vector3>& ee_pos,
00775                                                    const std::vector<hrp::Vector3>& cop_pos,
00776                                                    const std::vector<hrp::Matrix33>& ee_rot,
00777                                                    const std::vector<std::string>& ee_name,
00778                                                    const std::vector<double>& limb_gains,
00779                                                    const std::vector<double>& toeheel_ratio,
00780                                                    const hrp::Vector3& new_refzmp, const hrp::Vector3& ref_zmp,
00781                                                    const double total_fz, const double dt, const bool printp = true, const std::string& print_str = "",
00782                                                    const bool use_cop_distribution = true, const std::vector<bool> is_contact_list = std::vector<bool>())
00783     {
00784         size_t ee_num = ee_name.size();
00785         std::vector<double> alpha_vector(ee_num), fz_alpha_vector(ee_num);
00786         calcAlphaVectorFromCOPDistance(alpha_vector, fz_alpha_vector, cop_pos, ee_name, new_refzmp, ref_zmp);
00787         if (printp) {
00788             std::cerr << "[" << print_str << "] force moment distribution (Pinv)" << std::endl;
00789         }
00790 
00791         // ref_foot_force and ref_foot_moment should be set
00792         double norm_weight = 1e-7;
00793         double norm_moment_weight = 1e2;
00794         size_t total_wrench_dim = 5;
00795         size_t state_dim = 6*ee_num;
00796 
00797         // Temporarily set ref_foot_force and ref_foot_moment based on reference values
00798         {
00799             size_t total_wrench_dim = 5;
00800             //size_t total_wrench_dim = 3;
00801             hrp::dmatrix Wmat = hrp::dmatrix::Identity(state_dim/2, state_dim/2);
00802             hrp::dmatrix Gmat = hrp::dmatrix::Zero(total_wrench_dim, state_dim/2);
00803             for (size_t j = 0; j < ee_num; j++) {
00804                 if (total_wrench_dim == 3) {
00805                     Gmat(0,3*j+2) = 1.0;
00806                 } else {
00807                     for (size_t k = 0; k < 3; k++) Gmat(k,3*j+k) = 1.0;
00808                 }
00809             }
00810             for (size_t i = 0; i < total_wrench_dim; i++) {
00811                 for (size_t j = 0; j < ee_num; j++) {
00812                     if ( i == total_wrench_dim-2 ) { // Nx
00813                         Gmat(i,3*j+1) = -(cop_pos[j](2) - ref_zmp(2));
00814                         Gmat(i,3*j+2) = (cop_pos[j](1) - ref_zmp(1));
00815                     } else if ( i == total_wrench_dim-1 ) { // Ny
00816                         Gmat(i,3*j) = (cop_pos[j](2) - ref_zmp(2));
00817                         Gmat(i,3*j+2) = -(cop_pos[j](0) - ref_zmp(0));
00818                     }
00819                 }
00820             }
00821             for (size_t j = 0; j < ee_num; j++) {
00822                 for (size_t i = 0; i < 3; i++) {
00823                     if (i != 2 && ee_num == 2)
00824                         Wmat(i+j*3, i+j*3) = 0;
00825                     else
00826                         Wmat(i+j*3, i+j*3) = Wmat(i+j*3, i+j*3) * limb_gains[j];
00827                 }
00828             }
00829             if (printp) {
00830                 std::cerr << "[" << print_str << "]   Wmat(diag) = [";
00831                 for (size_t j = 0; j < ee_num; j++) {
00832                     for (size_t i = 0; i < 3; i++) {
00833                         std::cerr << Wmat(i+j*3, i+j*3) << " ";
00834                     }
00835                 }
00836                 std::cerr << "]" << std::endl;
00837             }
00838             hrp::dvector ret(state_dim/2);
00839             hrp::dvector total_wrench = hrp::dvector::Zero(total_wrench_dim);
00840             total_wrench(total_wrench_dim-3) = total_fz;
00841             calcWeightedLinearEquation(ret, Gmat, Wmat, total_wrench);
00842             for (size_t fidx = 0; fidx < ee_num; fidx++) {
00843                 ref_foot_force[fidx] = hrp::Vector3(ret(3*fidx), ret(3*fidx+1), ret(3*fidx+2));
00844                 ref_foot_moment[fidx] = hrp::Vector3::Zero();
00845             }
00846             // std::cerr << "GmatRef" << std::endl;
00847             // std::cerr << Gmat << std::endl;
00848             // std::cerr << "WmatRef" << std::endl;
00849             // std::cerr << Wmat << std::endl;
00850             // std::cerr << "ret" << std::endl;
00851             // std::cerr << ret << std::endl;
00852         }
00853         if (printp) {
00854             for (size_t i = 0; i < ee_num; i++) {
00855                 std::cerr << "[" << print_str << "]   "
00856                           << "ref_force  [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_force[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], "
00857                           << "ref_moment [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_moment[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
00858             }
00859         }
00860 
00861         // Calculate force/moment distribution matrix and vector
00862         //   We assume F = G f, calculate F and G. f is absolute here.
00863         //   1. Calculate F (total_wrench)
00864         hrp::dvector total_wrench = hrp::dvector::Zero(total_wrench_dim);
00865         hrp::Vector3 ref_total_force = hrp::Vector3::Zero();
00866         for (size_t fidx = 0; fidx < ee_num; fidx++) {
00867             double tmp_tau_x = -(cop_pos[fidx](2)-ref_zmp(2)) * ref_foot_force[fidx](1)
00868                 + (cop_pos[fidx](1)-ref_zmp(1)) * ref_foot_force[fidx](2)
00869                 + ref_foot_moment[fidx](0);
00870             total_wrench(3) -= tmp_tau_x;
00871             double tmp_tau_y = (cop_pos[fidx](2)-ref_zmp(2)) * ref_foot_force[fidx](0)
00872                 - (cop_pos[fidx](0)-ref_zmp(0)) * ref_foot_force[fidx](2)
00873                 + ref_foot_moment[fidx](1);
00874             total_wrench(4) -= tmp_tau_y;
00875             ref_total_force += ref_foot_force[fidx];
00876         }
00877         total_wrench(3) -= -(ref_zmp(2) - new_refzmp(2)) * ref_total_force(1) + (ref_zmp(1) - new_refzmp(1)) * ref_total_force(2);
00878         total_wrench(4) -= (ref_zmp(2) - new_refzmp(2)) * ref_total_force(0) - (ref_zmp(0) - new_refzmp(0)) * ref_total_force(2);
00879         //   2. Calculate G (Gmat)
00880         hrp::dmatrix Gmat = hrp::dmatrix::Zero(total_wrench_dim, state_dim);
00881         for (size_t j = 0; j < ee_num; j++) {
00882             for (size_t k = 0; k < total_wrench_dim; k++) Gmat(k,6*j+k) = 1.0;
00883         }
00884         for (size_t i = 0; i < total_wrench_dim; i++) {
00885             for (size_t j = 0; j < ee_num; j++) {
00886                 if ( i == 3 ) { // Nx
00887                     Gmat(i,6*j+1) = -(cop_pos[j](2) - new_refzmp(2));
00888                     Gmat(i,6*j+2) = (cop_pos[j](1) - new_refzmp(1));
00889                 } else if ( i == 4) { // Ny
00890                     Gmat(i,6*j) = (cop_pos[j](2) - new_refzmp(2));
00891                     Gmat(i,6*j+2) = -(cop_pos[j](0) - new_refzmp(0));
00892                 }
00893             }
00894         }
00895         // Calc rotation matrix to introduce local frame
00896         //   G = [tmpsubG_0, tmpsubG_1, ...]
00897         //   R = diag[tmpR_0,    tmpR_1,    ...]
00898         //   f = R f_l
00899         //   f : absolute, f_l : local
00900         //   G f = G R f_l
00901         //   G R = [tmpsubG_0 tmpR_0, tmpsubG_1 tmpR_1, ...] -> inserted to Gmat
00902         hrp::dmatrix tmpsubG = hrp::dmatrix::Zero(total_wrench_dim, 6);
00903         hrp::dmatrix tmpR = hrp::dmatrix::Zero(6,6);
00904         for (size_t ei = 0; ei < ee_num; ei++) {
00905             for (size_t i = 0; i < total_wrench_dim; i++) {
00906                 for (size_t j = 0; j < 6; j++) {
00907                     tmpsubG(i,j) = Gmat(i,6*ei+j);
00908                 }
00909             }
00910             for (size_t i = 0; i < 3; i++) {
00911                 for (size_t j = 0; j < 3; j++) {
00912                     tmpR(i,j) = tmpR(i+3,j+3) = ee_rot[ei](i,j);
00913                 }
00914             }
00915             tmpsubG = tmpsubG * tmpR;
00916             for (size_t i = 0; i < total_wrench_dim; i++) {
00917                 for (size_t j = 0; j < 6; j++) {
00918                     Gmat(i,6*ei+j) = tmpsubG(i,j);
00919                 }
00920             }
00921         }
00922 
00923         // Calc weighting matrix
00924         hrp::dmatrix Wmat = hrp::dmatrix::Zero(state_dim, state_dim);
00925         for (size_t j = 0; j < ee_num; j++) {
00926             for (size_t i = 0; i < 3; i++) {
00927                 Wmat(i+j*6, i+j*6) = fz_alpha_vector[j] * limb_gains[j];
00928                 Wmat(i+j*6+3, i+j*6+3) = (1.0/norm_moment_weight) * fz_alpha_vector[j] * limb_gains[j];
00929                 // Set local Y moment
00930                 //   If toeheel_ratio is 0, toe and heel contact and local Y moment should be 0.
00931                 if (i == 1) {
00932                     Wmat(i+j*6+3, i+j*6+3) = toeheel_ratio[j] * Wmat(i+j*6+3, i+j*6+3);
00933                 }
00934                 // In actual swing phase, X/Y momoment should be 0.
00935                 if (!is_contact_list.empty()) {
00936                   if (!is_contact_list[j]) Wmat(i+j*6+3, i+j*6+3) = 0;
00937                 }
00938             }
00939         }
00940         if (printp) {
00941             std::cerr << "[" << print_str << "]   newWmat(diag) = [";
00942             for (size_t j = 0; j < ee_num; j++) {
00943                 for (size_t i = 0; i < 6; i++) {
00944                     std::cerr << Wmat(i+j*6, i+j*6) << " ";
00945                 }
00946             }
00947             std::cerr << "]" << std::endl;
00948         }
00949         // std::cerr << "total_wrench" << std::endl;
00950         // std::cerr << total_wrench << std::endl;
00951         // std::cerr << "Gmat" << std::endl;
00952         // std::cerr << Gmat << std::endl;
00953         // std::cerr << "Wmat" << std::endl;
00954         // std::cerr << Wmat << std::endl;
00955 
00956         // Solve
00957         hrp::dvector ret(state_dim);
00958         calcWeightedLinearEquation(ret, Gmat, Wmat, total_wrench);
00959 
00960         // Extract force and moment with converting local frame -> absolute frame (ret is local frame)
00961         for (size_t fidx = 0; fidx < ee_num; fidx++) {
00962             ref_foot_force[fidx] += ee_rot[fidx] * hrp::Vector3(ret(6*fidx), ret(6*fidx+1), ret(6*fidx+2));
00963             ref_foot_moment[fidx] += ee_rot[fidx] * hrp::Vector3(ret(6*fidx+3), ret(6*fidx+4), ret(6*fidx+5));
00964         }
00965         if (printp) {
00966             for (size_t i = 0; i < ee_num; i++) {
00967                 std::cerr << "[" << print_str << "]   "
00968                           << "new_ref_force  [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_force[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], "
00969                           << "new_ref_moment [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_moment[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
00970             }
00971         }
00972     };
00973 
00974     void distributeZMPToForceMomentsPseudoInverse2 (std::vector<hrp::Vector3>& ref_foot_force, std::vector<hrp::Vector3>& ref_foot_moment,
00975                                                    const std::vector<hrp::Vector3>& ee_pos,
00976                                                    const std::vector<hrp::Vector3>& cop_pos,
00977                                                    const std::vector<hrp::Matrix33>& ee_rot,
00978                                                    const std::vector<std::string>& ee_name,
00979                                                    const std::vector<double>& limb_gains,
00980                                                    const std::vector<double>& toeheel_ratio,
00981                                                    const hrp::Vector3& new_refzmp, const hrp::Vector3& ref_zmp,
00982                                                    const hrp::Vector3& total_force, const hrp::Vector3& total_moment,
00983                                                    const std::vector<hrp::dvector6>& ee_forcemoment_distribution_weight,
00984                                                    const double total_fz, const double dt, const bool printp = true, const std::string& print_str = "")
00985     {
00986 #define FORCE_MOMENT_DIFF_CONTROL
00987 
00988         size_t ee_num = ee_name.size();
00989         std::vector<double> alpha_vector(ee_num), fz_alpha_vector(ee_num);
00990         calcAlphaVectorFromCOPDistance(alpha_vector, fz_alpha_vector, cop_pos, ee_name, new_refzmp, ref_zmp);
00991         if (printp) {
00992             std::cerr << "[" << print_str << "] force moment distribution (Pinv2) ";
00993 #ifdef FORCE_MOMENT_DIFF_CONTROL
00994             std::cerr << "(FORCE_MOMENT_DIFF_CONTROL)" << std::endl;
00995 #else
00996             std::cerr << "(NOT FORCE_MOMENT_DIFF_CONTROL)" << std::endl;
00997 #endif
00998         }
00999         // ref_foot_force and ref_foot_moment should be set
01000         size_t state_dim = 6*ee_num;
01001         if (printp) {
01002             std::cerr << "[" << print_str << "]   fz_alpha_vector [";
01003             for (size_t j = 0; j < ee_num; j++) {
01004                 std::cerr << fz_alpha_vector[j] << " ";
01005             }
01006             std::cerr << std::endl;
01007         }
01008 
01009         hrp::dvector total_wrench = hrp::dvector::Zero(6);
01010 #ifndef FORCE_MOMENT_DIFF_CONTROL
01011         total_wrench(0) = total_force(0);
01012         total_wrench(1) = total_force(1);
01013         total_wrench(2) = total_force(2);
01014 #endif
01015         total_wrench(3) = total_moment(0);
01016         total_wrench(4) = total_moment(1);
01017         total_wrench(5) = total_moment(2);
01018 #ifdef FORCE_MOMENT_DIFF_CONTROL
01019         for (size_t fidx = 0; fidx < ee_num; fidx++) {
01020             double tmp_tau_x = -(cop_pos[fidx](2)-new_refzmp(2)) * ref_foot_force[fidx](1) + (cop_pos[fidx](1)-new_refzmp(1)) * ref_foot_force[fidx](2);
01021             total_wrench(3) -= tmp_tau_x;
01022             double tmp_tau_y = (cop_pos[fidx](2)-new_refzmp(2)) * ref_foot_force[fidx](0) - (cop_pos[fidx](0)-new_refzmp(0)) * ref_foot_force[fidx](2);
01023             total_wrench(4) -= tmp_tau_y;
01024         }
01025 #endif
01026 
01027         hrp::dmatrix Wmat = hrp::dmatrix::Zero(state_dim, state_dim);
01028         hrp::dmatrix Gmat = hrp::dmatrix::Zero(6, state_dim);
01029         for (size_t j = 0; j < ee_num; j++) {
01030             for (size_t k = 0; k < 6; k++) Gmat(k,6*j+k) = 1.0;
01031         }
01032         for (size_t i = 0; i < 6; i++) {
01033             for (size_t j = 0; j < ee_num; j++) {
01034                 if ( i == 3 ) { // Nx
01035                     Gmat(i,6*j+1) = -(cop_pos[j](2) - new_refzmp(2));
01036                     Gmat(i,6*j+2) = (cop_pos[j](1) - new_refzmp(1));
01037                 } else if ( i == 4 ) { // Ny
01038                     Gmat(i,6*j) = (cop_pos[j](2) - new_refzmp(2));
01039                     Gmat(i,6*j+2) = -(cop_pos[j](0) - new_refzmp(0));
01040                 }
01041             }
01042         }
01043         for (size_t j = 0; j < ee_num; j++) {
01044             for (size_t i = 0; i < 3; i++) {
01045                 Wmat(i+j*6, i+j*6) = ee_forcemoment_distribution_weight[j][i] * fz_alpha_vector[j] * limb_gains[j];
01046                 //double norm_moment_weight = 1e2;
01047                 //Wmat(i+j*6+3, i+j*6+3) = ee_forcemoment_distribution_weight[j][i+3] * (1.0/norm_moment_weight) * fz_alpha_vector[j] * limb_gains[j];
01048                 Wmat(i+j*6+3, i+j*6+3) = ee_forcemoment_distribution_weight[j][i+3] * fz_alpha_vector[j] * limb_gains[j];
01049             }
01050         }
01051         if (printp) {
01052             std::cerr << "[" << print_str << "]   newWmat(diag) = [";
01053             for (size_t j = 0; j < ee_num; j++) {
01054                 for (size_t i = 0; i < 6; i++) {
01055                     std::cerr << Wmat(i+j*6, i+j*6) << " ";
01056                 }
01057             }
01058             std::cerr << std::endl;
01059             // std::cerr << "[" << print_str << "]   Gmat";
01060             // std::cerr << Gmat << std::endl;
01061             // std::cerr << "Wmat" << std::endl;
01062             // std::cerr << Wmat << std::endl;
01063         }
01064 
01065         hrp::dvector ret(state_dim);
01066         // Consider 6DOF total wrench (Fx, Fy, Fz, Mx, My, Mz)
01067         hrp::dmatrix selection_matrix = hrp::dmatrix::Identity(6,6);
01068         // Consdier 3DOF total wrench (Fz, Mx, My)
01069 //         hrp::dmatrix selection_matrix = hrp::dmatrix::Zero(3,6);
01070 //         selection_matrix(0,2) = 1.0;
01071 //         selection_matrix(1,3) = 1.0;
01072 //         selection_matrix(2,4) = 1.0;
01073         {
01074             hrp::dvector selected_total_wrench = hrp::dvector::Zero(selection_matrix.rows());
01075             hrp::dmatrix selected_Gmat = hrp::dmatrix::Zero(selection_matrix.rows(), Gmat.cols());
01076             selected_total_wrench = selection_matrix * total_wrench;
01077             selected_Gmat = selection_matrix * Gmat;
01078             calcWeightedLinearEquation(ret, selected_Gmat, Wmat, selected_total_wrench);
01079         }
01080 
01081         if (printp) {
01082             hrp::dvector tmpretv(total_wrench.size());
01083             tmpretv = Gmat * ret - total_wrench;
01084             std::cerr << "[" << print_str << "]   "
01085                       << "test_diff " << tmpretv.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N][Nm]" << std::endl;
01086             std::cerr << "[" << print_str << "]   "
01087                       << "total_wrench " << total_wrench.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N][Nm]" << std::endl;
01088             for (size_t i = 0; i < ee_num; i++) {
01089                 std::cerr << "[" << print_str << "]   "
01090                           << "ref_force  [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_force[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], "
01091                           << "ref_moment [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_moment[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
01092             }
01093             for (size_t i = 0; i < ee_num; i++) {
01094 #ifdef FORCE_MOMENT_DIFF_CONTROL
01095                 std::cerr << "[" << print_str << "]   "
01096                           << "dif_ref_force  [" << ee_name[i] << "] " << hrp::Vector3(hrp::Vector3(ret(6*i), ret(6*i+1), ret(6*i+2))).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], "
01097                           << "dif_ref_moment [" << ee_name[i] << "] " << hrp::Vector3(hrp::Vector3(ret(6*i+3), ret(6*i+4), ret(6*i+5))).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
01098 #else
01099                 std::cerr << "[" << print_str << "]   "
01100                           << "dif_ref_force  [" << ee_name[i] << "] " << hrp::Vector3(hrp::Vector3(ret(6*i), ret(6*i+1), ret(6*i+2))-ref_foot_force[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], "
01101                           << "dif_ref_moment [" << ee_name[i] << "] " << hrp::Vector3(hrp::Vector3(ret(6*i+3), ret(6*i+4), ret(6*i+5))-ref_foot_moment[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
01102 #endif
01103             }
01104         }
01105         for (size_t fidx = 0; fidx < ee_num; fidx++) {
01106 #ifdef FORCE_MOMENT_DIFF_CONTROL
01107             ref_foot_force[fidx] += hrp::Vector3(ret(6*fidx), ret(6*fidx+1), ret(6*fidx+2));
01108             ref_foot_moment[fidx] += hrp::Vector3(ret(6*fidx+3), ret(6*fidx+4), ret(6*fidx+5));
01109 #else
01110             ref_foot_force[fidx] = hrp::Vector3(ret(6*fidx), ret(6*fidx+1), ret(6*fidx+2));
01111             ref_foot_moment[fidx] = hrp::Vector3(ret(6*fidx+3), ret(6*fidx+4), ret(6*fidx+5));
01112 #endif
01113         }
01114         if (printp){
01115             for (size_t i = 0; i < ee_num; i++) {
01116                 std::cerr << "[" << print_str << "]   "
01117                           << "new_ref_force  [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_force[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], "
01118                           << "new_ref_moment [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_moment[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
01119             }
01120         }
01121     };
01122 
01123   double calcCrossProduct(const Eigen::Vector2d& a, const Eigen::Vector2d& b, const Eigen::Vector2d& o)
01124   {
01125     return (a(0) - o(0)) * (b(1) - o(1)) - (a(1) - o(1)) * (b(0) - o(0));
01126   };
01127 
01128   projected_point_region calcProjectedPoint(Eigen::Vector2d& ret, const Eigen::Vector2d& target, const Eigen::Vector2d& a, const Eigen::Vector2d& b){
01129     Eigen::Vector2d v1 = target - b;
01130     Eigen::Vector2d v2 = a - b;
01131     double v2_norm = v2.norm();
01132     if ( v2_norm == 0 ) {
01133         ret = a;
01134         return LEFT;
01135     } else {
01136         double ratio = v1.dot(v2) / (v2_norm * v2_norm);
01137         if (ratio < 0){
01138             ret = b;
01139             return RIGHT;
01140         } else if (ratio > 1){
01141             ret = a;
01142             return LEFT;
01143         } else {
01144             ret = b + ratio * v2;
01145             return MIDDLE;
01146         }
01147     }
01148   };
01149 };
01150 
01151 #endif // ZMP_DISTRIBUTOR_H


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