testZMPDistributor.cpp
Go to the documentation of this file.
00001 /* -*- coding:utf-8-unix; mode:c++; -*- */
00002 
00003 #include "ZMPDistributor.h"
00004 /* samples */
00005 #include <stdio.h>
00006 #include <cstdio>
00007 #include <iostream>
00008 #include "hrpsys/util/Hrpsys.h" // added for QNX compile
00009 
00010 class testZMPDistributor
00011 {
00012 protected:
00013     double dt; // [s]
00014     double total_fz; // [N]
00015     SimpleZMPDistributor* szd;
00016     bool use_gnuplot;
00017     enum {EEFM, EEFMQP, EEFMQP2, EEFMQPCOP, EEFMQPCOP2} distribution_algorithm;
00018     size_t sleep_msec;
00019     std::vector<hrp::Vector3> leg_pos;
00020     std::vector<hrp::Vector3> ee_pos;
00021     std::vector<hrp::Vector3> cop_pos;
00022     std::vector<hrp::Matrix33> ee_rot;
00023     std::vector<std::vector<Eigen::Vector2d> > fs;
00024 private:
00025     void gen_and_plot ()
00026     {
00027         parse_params();
00028         std::cerr << "distribution_algorithm = ";
00029         if (distribution_algorithm==EEFM) std::cerr << "EEFM";
00030         else if (distribution_algorithm==EEFMQP) std::cerr << "EEFMQP";
00031         else if (distribution_algorithm==EEFMQP2) std::cerr << "EEFMQP(cop_distribution)";
00032         else if (distribution_algorithm==EEFMQPCOP) std::cerr << "EEFMQPCOP";
00033         else if (distribution_algorithm==EEFMQPCOP2) std::cerr << "EEFMQPCOP2";
00034         else std::cerr << "None?";
00035         std::cerr << ", sleep_msec = " << sleep_msec << "[msec]" << std::endl;
00036         szd->print_vertices("");
00037         szd->print_params(std::string(""));
00038         //
00039         std::vector<double> x_vec;
00040         double txx = -0.25;
00041         while (txx < 0.25) {
00042             x_vec.push_back(txx);
00043             txx += 0.05;
00044         };
00045         std::vector<double> y_vec;
00046         double tyy = -0.24;
00047         while (tyy < 0.25) {
00048             y_vec.push_back(tyy);
00049             tyy += 0.01;
00050         };
00051         std::vector<hrp::Vector3> refzmp_vec;
00052         for (size_t xx = 0; xx < x_vec.size(); xx++) {
00053             for (size_t yy = 0; yy < y_vec.size(); yy++) {
00054                 refzmp_vec.push_back(hrp::Vector3(x_vec[xx], y_vec[yy], 0.0));
00055             }
00056         }
00057         //
00058 
00059         // plot
00060         FILE* gp;
00061         FILE* gp_m;
00062         FILE* gp_f;
00063         FILE* gp_a;
00064         if (use_gnuplot) {
00065             gp = popen("gnuplot", "w");
00066             gp_m = popen("gnuplot", "w");
00067             gp_f = popen("gnuplot", "w");
00068             gp_a = popen("gnuplot", "w");
00069         }
00070         std::string fname_fm("/tmp/plot-fm.dat");
00071         FILE* fp_fm = fopen(fname_fm.c_str(), "w");
00072         std::vector<std::string> names;
00073         names.push_back("rleg");
00074         names.push_back("lleg");
00075         std::vector<double> limb_gains(names.size(), 1.0);
00076         std::vector<double> toeheel_ratio(names.size(), 1.0);
00077         std::vector<hrp::Vector3> ref_foot_force(names.size(), hrp::Vector3::Zero()), ref_foot_moment(names.size(), hrp::Vector3::Zero());
00078         std::vector<std::vector<Eigen::Vector2d> > fs;
00079         szd->get_vertices(fs);
00080         //
00081         std::ostringstream oss_foot_params("");
00082         oss_foot_params << "'/tmp/plotrleg.dat' using 1:2:3 with lines title 'rleg', "
00083                         << "'/tmp/plotlleg.dat' using 1:2:3 with lines title 'lleg', "
00084                         << "'/tmp/plotrleg-cop.dat' using 1:2:3 with points title 'rleg cop', "
00085                         << "'/tmp/plotlleg-cop.dat' using 1:2:3 with points title 'lleg cop', ";
00086         std::string foot_params_str = oss_foot_params.str();
00087         //
00088         for (size_t i = 0; i < refzmp_vec.size(); i++) {
00089             double alpha = szd->calcAlpha(refzmp_vec[i], ee_pos, ee_rot, names);
00090             if (distribution_algorithm == EEFMQP) {
00091                 szd->distributeZMPToForceMoments(ref_foot_force, ref_foot_moment,
00092                                                  ee_pos, cop_pos, ee_rot, names, limb_gains, toeheel_ratio,
00093                                                  refzmp_vec[i], refzmp_vec[i],
00094                                                  total_fz, dt);
00095             } else if (distribution_algorithm == EEFMQP || distribution_algorithm == EEFMQP2) {
00096                 szd->distributeZMPToForceMomentsQP(ref_foot_force, ref_foot_moment,
00097                                                    ee_pos, cop_pos, ee_rot, names, limb_gains, toeheel_ratio,
00098                                                    refzmp_vec[i], refzmp_vec[i],
00099                                                    total_fz, dt, true, "", (distribution_algorithm == EEFMQP2));
00100             } else if (distribution_algorithm == EEFMQPCOP) {
00101                 szd->distributeZMPToForceMomentsPseudoInverse(ref_foot_force, ref_foot_moment,
00102                                                               ee_pos, cop_pos, ee_rot, names, limb_gains, toeheel_ratio,
00103                                                               refzmp_vec[i], refzmp_vec[i],
00104                                                               total_fz, dt, true, "");
00105             } else if (distribution_algorithm == EEFMQPCOP2) {
00106                 // std::vector<double> ee_forcemoment_distribution_weight(names.size(), 1.0);
00107                 // szd->distributeZMPToForceMomentsPseudoInverse2(ref_foot_force, ref_foot_moment,
00108                 //                                                ee_pos, cop_pos, ee_rot, names, limb_gains, toeheel_ratio,
00109                 //                                                refzmp_vec[i], refzmp_vec[i],
00110                 //                                                total_fz, dt, true, "");
00111             }
00112             for (size_t j = 0; j < fs.size(); j++) {
00113                 std::string fname("/tmp/plot"+names[j]+".dat");
00114                 FILE* fp = fopen(fname.c_str(), "w");
00115                 for (size_t i = 0; i < fs[j].size(); i++) {
00116                     hrp::Vector3 tmpf(hrp::Vector3(ee_rot[j] * hrp::Vector3(fs[j][i](0), fs[j][i](1), 0) + ee_pos[j]));
00117                     fprintf(fp, "%f %f %f\n", tmpf(0), tmpf(1), tmpf(2));
00118                 }
00119                 hrp::Vector3 tmpf(hrp::Vector3(ee_rot[j] * hrp::Vector3(fs[j][0](0), fs[j][0](1), 0) + ee_pos[j]));
00120                 fprintf(fp, "%f %f %f\n", tmpf(0), tmpf(1), tmpf(2));
00121                 fclose(fp);
00122             }
00123             for (size_t j = 0; j < fs.size(); j++) {
00124                 std::string fname("/tmp/plot"+names[j]+"-cop.dat");
00125                 FILE* fp = fopen(fname.c_str(), "w");
00126                 fprintf(fp, "%f %f %f\n", ee_pos[j](0), ee_pos[j](1), ee_pos[j](2));
00127                 fclose(fp);
00128             }
00129             for (size_t j = 0; j < fs.size(); j++) {
00130                 std::string fname("/tmp/plot"+names[j]+"fm.dat");
00131                 FILE* fp = fopen(fname.c_str(), "w");
00132                 hrp::Vector3 cop;
00133                 cop(0) = -ref_foot_moment[j](1)/ref_foot_force[j](2) + cop_pos[j](0);
00134                 cop(1) =  ref_foot_moment[j](0)/ref_foot_force[j](2) + cop_pos[j](1);
00135                 fprintf(fp, "%f %f 0\n", cop(0), cop(1));
00136                 fprintf(fp, "%f %f %f\n", cop(0), cop(1), ref_foot_force[j](2));
00137                 fclose(fp);
00138             }
00139             fprintf(fp_fm, "%f %f %f %f %f %f %f %f %f\n",
00140                     refzmp_vec[i](0), refzmp_vec[i](1),
00141                     ref_foot_force[0](2), ref_foot_force[1](2),
00142                     ref_foot_moment[0](0), ref_foot_moment[1](0),
00143                     ref_foot_moment[0](1), ref_foot_moment[1](1),
00144                     alpha);
00145             {
00146                 std::string fname("/tmp/plotzmp.dat");
00147                 FILE* fp = fopen(fname.c_str(), "w");
00148                 fprintf(fp, "%f %f %f\n", refzmp_vec[i](0), refzmp_vec[i](1), refzmp_vec[i](2));
00149                 fclose(fp);
00150             }
00151             if (use_gnuplot) {
00152                 std::ostringstream oss("");
00153                 oss << "splot [-0.5:0.5][-0.5:0.5][-1:1000] " << foot_params_str
00154                     << "'/tmp/plotrlegfm.dat' using 1:2:3 with lines title 'rleg fm' lw 5, "
00155                     << "'/tmp/plotllegfm.dat' using 1:2:3 with lines title 'lleg fm' lw 5, "
00156                     << "'/tmp/plotzmp.dat' using 1:2:3 with points title 'zmp' lw 10";
00157                 fprintf(gp, "%s\n", oss.str().c_str());
00158                 fflush(gp);
00159                 usleep(1000*sleep_msec);
00160             }
00161         }
00162         fclose(fp_fm);
00163         if (use_gnuplot) {
00164             std::ostringstream oss("");
00165             oss << "splot [-0.5:0.5][-0.5:0.5][-100:100] " << foot_params_str
00166                 << "'/tmp/plot-fm.dat' using 1:2:5 with points title 'rleg nx' lw 5, "
00167                 << "'/tmp/plot-fm.dat' using 1:2:6 with points title 'lleg nx' lw 5, "
00168                 << "'/tmp/plot-fm.dat' using 1:2:7 with points title 'rleg ny' lw 5, "
00169                 << "'/tmp/plot-fm.dat' using 1:2:8 with points title 'lleg ny' lw 5";
00170             fprintf(gp_m, "%s\n", oss.str().c_str());
00171             fflush(gp_m);
00172             oss.str("");
00173             oss << "splot [-0.5:0.5][-0.5:0.5][-50:" << total_fz*1.1 << "] " << foot_params_str
00174                 << "'/tmp/plot-fm.dat' using 1:2:3 with points title 'rleg fz' lw 5, "
00175                 << "'/tmp/plot-fm.dat' using 1:2:4 with points title 'lleg fz' lw 5";
00176             fprintf(gp_f, "%s\n", oss.str().c_str());
00177             fflush(gp_f);
00178             oss.str("");
00179             oss << "splot [-0.5:0.5][-0.5:0.5][-0.1:1.1] " << foot_params_str
00180                 << "'/tmp/plot-fm.dat' using 1:2:9 with points title 'alpha' lw 5";
00181             fprintf(gp_a, "%s\n", oss.str().c_str());
00182             fflush(gp_a);
00183             double tmp;
00184             std::cin >> tmp;
00185             pclose(gp);
00186             pclose(gp_m);
00187             pclose(gp_f);
00188             pclose(gp_a);
00189         }
00190     };
00191 public:
00192     std::vector<std::string> arg_strs;
00193     testZMPDistributor(const double _dt) : dt(_dt), distribution_algorithm(EEFMQP), use_gnuplot(true), sleep_msec(100)
00194     {
00195         szd = new SimpleZMPDistributor(_dt);
00196     };
00197     virtual ~testZMPDistributor()
00198     {
00199         if (szd != NULL) {
00200             delete szd;
00201             szd = NULL;
00202         }
00203     };
00204 
00205     void parse_params ()
00206     {
00207       for (int i = 0; i < arg_strs.size(); ++ i) {
00208           if ( arg_strs[i]== "--distribution-algorithm" ) {
00209               if (++i < arg_strs.size()) {
00210                   if (arg_strs[i]=="EEFM") {
00211                       distribution_algorithm = EEFM;
00212                   } else if (arg_strs[i]=="EEFMQP") {
00213                       distribution_algorithm = EEFMQP;
00214                   } else if (arg_strs[i]=="EEFMQP2") {
00215                       distribution_algorithm = EEFMQP2;
00216                   } else if (arg_strs[i]=="EEFMQPCOP") {
00217                       distribution_algorithm = EEFMQPCOP;
00218                   } else if (arg_strs[i]=="EEFMQPCOP") {
00219                       distribution_algorithm = EEFMQPCOP;
00220                   } else if (arg_strs[i]=="EEFMQPCOP2") {
00221                       distribution_algorithm = EEFMQPCOP2;
00222                   } else {
00223                       distribution_algorithm = EEFM;
00224                   }
00225               }
00226           } else if ( arg_strs[i]== "--sleep-msec" ) {
00227               if (++i < arg_strs.size()) sleep_msec = atoi(arg_strs[i].c_str());
00228           } else if ( arg_strs[i]== "--use-gnuplot" ) {
00229               if (++i < arg_strs.size()) use_gnuplot = (arg_strs[i]=="true");
00230           }
00231       }
00232     };
00233 
00234     void test0 ()
00235     {
00236         std::cerr << "test0 : Default foot pos" << std::endl;
00237         ee_pos = leg_pos;
00238         cop_pos = leg_pos;
00239         ee_rot.push_back(hrp::Matrix33::Identity());
00240         ee_rot.push_back(hrp::Matrix33::Identity());
00241         gen_and_plot();
00242     };
00243 
00244     void test1 ()
00245     {
00246         std::cerr << "test1 : Fwd foot pos" << std::endl;
00247         ee_pos = leg_pos;
00248         ee_pos[0] = ee_pos[0] + hrp::Vector3(0.05,0,0);
00249         ee_pos[1] = ee_pos[1] + hrp::Vector3(-0.05,0,0);
00250         cop_pos = ee_pos;
00251         ee_rot.push_back(hrp::Matrix33::Identity());
00252         ee_rot.push_back(hrp::Matrix33::Identity());
00253         gen_and_plot();
00254     };
00255 
00256     void test2 ()
00257     {
00258         std::cerr << "test2 : Rot foot pos" << std::endl;
00259         ee_pos = leg_pos;
00260         ee_pos[0] = ee_pos[0] + hrp::Vector3(0.02,0,0);
00261         ee_pos[1] = ee_pos[1] + hrp::Vector3(-0.02,0,0);
00262         cop_pos = ee_pos;
00263         hrp::Matrix33 tmpr;
00264         tmpr = hrp::rotFromRpy(hrp::Vector3(0,0,-15*M_PI/180.0));
00265         ee_rot.push_back(tmpr);
00266         tmpr = hrp::rotFromRpy(hrp::Vector3(0,0,15*M_PI/180.0));
00267         ee_rot.push_back(tmpr);
00268         gen_and_plot();
00269     };
00270 };
00271 
00272 class testZMPDistributorHRP2JSK : public testZMPDistributor
00273 {
00274  public:
00275     testZMPDistributorHRP2JSK () : testZMPDistributor(0.004)
00276         {
00277             total_fz = 56*9.8066;
00278             szd->set_leg_inside_margin(0.070104);
00279             szd->set_leg_outside_margin(0.070104);
00280             szd->set_leg_front_margin(0.137525);
00281             szd->set_leg_rear_margin(0.106925);
00282             szd->set_vertices_from_margin_params();
00283             leg_pos.push_back(hrp::Vector3(0,-0.105,0));
00284             leg_pos.push_back(hrp::Vector3(0,0.105,0));
00285         };
00286 };
00287 
00288 class testZMPDistributorJAXON_RED : public testZMPDistributor
00289 {
00290  public:
00291     testZMPDistributorJAXON_RED () : testZMPDistributor(0.002)
00292         {
00293             total_fz = 130.442*9.8066;
00294             szd->set_leg_inside_margin(0.055992);
00295             szd->set_leg_outside_margin(0.075992);
00296             szd->set_leg_front_margin(0.133242);
00297             szd->set_leg_rear_margin(0.100445);
00298             szd->set_vertices_from_margin_params();
00299             leg_pos.push_back(hrp::Vector3(0,-0.100,0));
00300             leg_pos.push_back(hrp::Vector3(0,0.100,0));
00301         };
00302 };
00303 
00304 void print_usage ()
00305 {
00306     std::cerr << "Usage : testZMPDistributor [robot-name] [test-type] [option]" << std::endl;
00307     std::cerr << " [robot-name] should be: --hrp2jsk, --jaxon_red" << std::endl;
00308     std::cerr << " [test-type] should be:" << std::endl;
00309     std::cerr << "  --test0 : Default foot pos" << std::endl;
00310     std::cerr << "  --test1 : Fwd foot pos" << std::endl;
00311     std::cerr << "  --test2 : Rot foot pos" << std::endl;
00312 };
00313 
00314 int main(int argc, char* argv[])
00315 {
00316     int ret = 0;
00317     if (argc >= 3) {
00318         testZMPDistributor* tzd = NULL;
00319         if (std::string(argv[1]) == "--hrp2jsk") {
00320             tzd = new testZMPDistributorHRP2JSK();
00321         } else if (std::string(argv[1]) == "--jaxon_red") {
00322             tzd = new testZMPDistributorJAXON_RED();
00323         } else {
00324             print_usage();
00325             ret = 1;
00326         }
00327         if (tzd != NULL) {
00328             for (int i = 2; i < argc; ++ i) {
00329                 tzd->arg_strs.push_back(std::string(argv[i]));
00330             }
00331             if (std::string(argv[2]) == "--test0") {
00332                 tzd->test0();
00333             } else if (std::string(argv[2]) == "--test1") {
00334                 tzd->test1();
00335             } else if (std::string(argv[2]) == "--test2") {
00336                 tzd->test2();
00337             } else {
00338                 print_usage();
00339                 ret = 1;
00340             }
00341             delete tzd;
00342         }
00343     } else {
00344         print_usage();
00345         ret = 1;
00346     }
00347     return ret;
00348 }
00349 


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