testPreviewController.cpp
Go to the documentation of this file.
00001 /* -*- coding:utf-8-unix; mode:c++; -*- */
00002 #include "PreviewController.h"
00003 
00004 using namespace hrp;
00005 using namespace rats;
00006 
00007 struct gait_parameter /* for test */
00008 {
00009   double tm;
00010   hrp::Vector3 ref_zmp;
00011   gait_parameter (const double _tm, const hrp::Vector3& _ref_zmp)
00012     : tm(_tm), ref_zmp(_ref_zmp) {};
00013 };
00014 
00015 #include<cstdio>
00016 
00017 int main(int argc, char* argv[])
00018 {
00019   /* this is c++ version example of test-preview-filter1-modified in euslib/jsk/preview.l*/
00020   bool use_gnuplot = true;
00021   if (argc >= 2) {
00022       if ( std::string(argv[1])== "--use-gnuplot" ) {
00023           use_gnuplot = (std::string(argv[2])=="true");
00024       }
00025   }
00026 
00027   double dt = 0.01, max_tm = 8.0;
00028   std::queue<hrp::Vector3> ref_zmp_list;
00029   std::deque<double> tm_list;
00030   for (size_t i = 0; i < static_cast<size_t>(round(max_tm / dt)); i++) {
00031     double tmp_tm = i * dt;
00032     tm_list.push_back(tmp_tm);
00033     hrp::Vector3 v;
00034     if (tmp_tm < 2) {
00035       v << 0, 0, 0;
00036     } else if (tmp_tm < 4) {
00037       v << -0.02, 0.02, 0;
00038     } else if (tmp_tm < 6) {
00039       v << 0.02, -0.02, 0;
00040     } else {
00041       v << 0, -0.02, 0;
00042     }
00043     ref_zmp_list.push(v);
00044   }
00045 
00046   //preview_dynamics_filter<preview_control> df(dt, 0.8, ref_zmp_list.front());
00047   preview_dynamics_filter<extended_preview_control> df(dt, 0.8, ref_zmp_list.front());
00048   std::string fname("/tmp/plot.dat");
00049   FILE* fp = fopen(fname.c_str(), "w");  
00050   double cart_zmp[3], refzmp[3];
00051   bool r = true;
00052   size_t index = 0;
00053   while (r) {
00054     hrp::Vector3 p, x;
00055     std::vector<hrp::Vector3> qdata;
00056     r = df.update(p, x, qdata, ref_zmp_list.front(), qdata, !ref_zmp_list.empty());
00057     if (r) {
00058       index++;
00059       df.get_cart_zmp(cart_zmp);
00060       df.get_current_refzmp(refzmp);
00061       fprintf(fp, "%f %f %f %f %f %f %f\n",
00062               tm_list[index],
00063               cart_zmp[0], /* zmpx ;; this zmp is "zmp as a table-cart model" */
00064               x[0], /* cogy */
00065               refzmp[0], /* refzmpx */
00066               cart_zmp[1], /* zmpy ;; this zmp is "zmp as a table-cart model" */
00067               x[1], /* cogy */
00068               refzmp[1] /* refzmpy */
00069               );
00070     } else if ( !ref_zmp_list.empty() ) r = true;
00071     if (!ref_zmp_list.empty()) ref_zmp_list.pop();
00072   }
00073   fclose(fp);
00074   if (use_gnuplot) {
00075   FILE* gp[3];
00076   std::string titles[2] = {"X", "Y"};
00077   for (size_t ii = 0; ii < 2; ii++) {
00078     gp[ii] = popen("gnuplot", "w");
00079     fprintf(gp[ii], "set title \"%s\"\n", titles[ii].c_str());
00080     fprintf(gp[ii], "plot \"%s\" using 1:%zu with lines title \"cart-table zmp\"\n", fname.c_str(), ( ii * 3 + 2));
00081     fprintf(gp[ii], "replot \"%s\" using 1:%zu with lines title \"cog\"\n", fname.c_str(), ( ii * 3 + 3));
00082     fprintf(gp[ii], "replot \"%s\" using 1:%zu with lines title \"refzmp\"\n", fname.c_str(), ( ii * 3 + 4));
00083     fflush(gp[ii]);
00084   }
00085   double tmp;
00086   std::cin >> tmp;
00087   for (size_t j = 0; j < 2; j++) pclose(gp[j]);
00088   }
00089   return 0;
00090 }


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