testImpedanceOutputGenerator.cpp
Go to the documentation of this file.
00001 /* -*- coding:utf-8-unix; mode:c++; -*- */
00002 
00003 #include "ImpedanceOutputGenerator.h"
00004 /* samples */
00005 #include <stdio.h>
00006 #include <cstdio>
00007 #include <iostream>
00008 #include <vector>
00009 
00010 #ifndef rad2deg
00011 #define rad2deg(rad) (rad * 180 / M_PI)
00012 #endif
00013 #ifndef deg2rad
00014 #define deg2rad(deg) (deg * M_PI / 180)
00015 #endif
00016 
00017 class testImpedanceOutputGenerator
00018 {
00019 protected:
00020     double dt; /* [s] */
00021     ImpedanceOutputGenerator imp;
00022     bool use_gnuplot;
00023     void gen_pattern_and_plot (const std::vector<hrp::Vector3>& force_diff_vec,
00024                                const std::vector<hrp::Vector3>& moment_diff_vec,
00025                                const std::vector<hrp::Vector3>& target_p0_vec,
00026                                const std::vector<hrp::Matrix33>& target_r0_vec,
00027                                const std::vector<double>& time_vec)
00028     {
00029         parse_params();
00030         std::string fname("/tmp/plot-imp.dat");
00031         FILE* fp = fopen(fname.c_str(), "w");
00032         for (size_t i = 0; i < time_vec.size();i++) {
00033             imp.target_p0 = target_p0_vec[i];
00034             imp.target_r0 = target_r0_vec[i];
00035             imp.current_p1 = imp.output_p1;
00036             imp.current_r1 = imp.output_r1;
00037             hrp::Vector3 vel_p, vel_r;
00038             hrp::Matrix33 eeR = hrp::Matrix33::Identity();
00039             imp.calcTargetVelocity(vel_p, vel_r,
00040                                    eeR, force_diff_vec[i], moment_diff_vec[i], dt);
00041             hrp::Vector3 output_rot, target_rot;
00042             rats::difference_rotation(output_rot, hrp::Matrix33::Identity(), imp.output_r1);
00043             rats::difference_rotation(target_rot, hrp::Matrix33::Identity(), imp.target_r1);
00044             fprintf(fp, "%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
00045                     time_vec[i],
00046                     imp.output_p1(0), imp.output_p1(1), imp.output_p1(2),
00047                     imp.target_p1(0), imp.target_p1(1), imp.target_p1(2),
00048                     force_diff_vec[i](0)/imp.K_p, force_diff_vec[i](1)/imp.K_p, force_diff_vec[i](2)/imp.K_p,
00049                     rad2deg(output_rot(0)), rad2deg(output_rot(1)), rad2deg(output_rot(2)),
00050                     rad2deg(target_rot(0)), rad2deg(target_rot(1)), rad2deg(target_rot(2)),
00051                     rad2deg(moment_diff_vec[i](0)/imp.K_r), rad2deg(moment_diff_vec[i](1)/imp.K_r), rad2deg(moment_diff_vec[i](2)/imp.K_r)
00052                     );
00053         }
00054         fclose(fp);
00055         if (use_gnuplot) {
00056         // plot
00057         std::string titles[3] = {"X", "Y", "Z"};
00058         //   plot pos
00059         FILE* gp_pos = popen("gnuplot", "w");
00060         fprintf(gp_pos, "set multiplot layout 3, 1 title 'Pos results'\n");
00061         for (size_t ii = 0; ii < 3; ii++) {
00062             fprintf(gp_pos, "set xlabel 'Time [s]'\n");
00063             fprintf(gp_pos, "set ylabel 'pos %s [m]'\n", titles[ii].c_str());
00064             fprintf(gp_pos, "plot '/tmp/plot-imp.dat' using 1:%d with lines title 'cur pos(%s)' lw 4, '/tmp/plot-imp.dat' using 1:%d with lines title 'tgt pos(%s)' lw 3, '/tmp/plot-imp.dat' using 1:%d with lines title 'force_diff/K(%s)' lw 2\n",
00065                     ii+2, titles[ii].c_str(), ii+2+3, titles[ii].c_str(), ii+2+3*2, titles[ii].c_str());
00066         }
00067         fflush(gp_pos);
00068         //   plot rot
00069         FILE* gp_rot = popen("gnuplot", "w");
00070         fprintf(gp_rot, "set multiplot layout 3, 1 title 'Rot results'\n");
00071         for (size_t ii = 0; ii < 3; ii++) {
00072             fprintf(gp_rot, "set xlabel 'Time [s]'\n");
00073             fprintf(gp_rot, "set ylabel 'rot %s [deg]'\n", titles[ii].c_str());
00074             fprintf(gp_rot, "plot '/tmp/plot-imp.dat' using 1:%d with lines title 'cur rot(%s)' lw 4, '/tmp/plot-imp.dat' using 1:%d with lines title 'tgt rot(%s)' lw 3, '/tmp/plot-imp.dat' using 1:%d with lines title 'moment_diff/K(%s)' lw 2\n",
00075                     ii+2+9, titles[ii].c_str(), ii+2+3+9, titles[ii].c_str(), ii+2+3*2+9, titles[ii].c_str());
00076         }
00077         fflush(gp_rot);
00078         double tmp;
00079         std::cin >> tmp;
00080         pclose(gp_pos);
00081         pclose(gp_rot);
00082         }
00083     };
00084 public:
00085     std::vector<std::string> arg_strs;
00086     testImpedanceOutputGenerator (const double _dt = 0.004) : dt(_dt), imp(), use_gnuplot(true) {};
00087     void test0 ()
00088     {
00089         std::cerr << "test0 : Set ref force" << std::endl;
00090         double tm = 0.0, total_tm = 4.0;
00091         std::vector<double> time_vec;
00092         std::vector<hrp::Vector3> force_diff_vec, moment_diff_vec, target_p0_vec;
00093         std::vector<hrp::Matrix33> target_r0_vec;
00094         for (size_t i = 0; i < static_cast<size_t>(total_tm/dt);i++) {
00095             time_vec.push_back(tm);
00096             force_diff_vec.push_back((i*dt < total_tm * 0.2 ? hrp::Vector3::Zero() : hrp::Vector3(10,-20,30)));
00097             moment_diff_vec.push_back((i*dt < total_tm * 0.3 ? hrp::Vector3::Zero() : hrp::Vector3(5,-10,15)));
00098             target_p0_vec.push_back(hrp::Vector3::Zero());
00099             target_r0_vec.push_back(hrp::Matrix33::Identity());
00100             tm += dt;
00101         }
00102         gen_pattern_and_plot (force_diff_vec, moment_diff_vec, target_p0_vec, target_r0_vec, time_vec);
00103     };
00104     void test1 ()
00105     {
00106         std::cerr << "test1 : Move pos and rot" << std::endl;
00107         double tm = 0.0, total_tm = 1.0;
00108         std::vector<double> time_vec;
00109         std::vector<hrp::Vector3> force_diff_vec, moment_diff_vec, target_p0_vec;
00110         std::vector<hrp::Matrix33> target_r0_vec;
00111         //imp.M_p = 0.0; imp.M_r = 0.0;
00112         for (size_t i = 0; i < static_cast<size_t>(total_tm/dt);i++) {
00113             time_vec.push_back(tm);
00114             force_diff_vec.push_back(hrp::Vector3::Zero());
00115             moment_diff_vec.push_back(hrp::Vector3::Zero());
00116             double ratio = (i*dt < total_tm * 0.3 ? i*dt/(total_tm * 0.3) : 1.0);
00117             target_p0_vec.push_back(hrp::Vector3((1-ratio)*hrp::Vector3::Zero()+ratio*hrp::Vector3(0.01,-0.02,0.03)));
00118             hrp::Vector3 tmpv(hrp::Vector3((1-ratio)*hrp::Vector3::Zero() + ratio*hrp::Vector3(0.1,-0.2,0.3)));
00119             Eigen::AngleAxis<double> tmpr;
00120             if (tmpv.norm() != 0.0) {
00121                 tmpr = Eigen::AngleAxis<double>(tmpv.norm(), tmpv.normalized());
00122             } else {
00123                 tmpr = hrp::Matrix33::Identity();
00124             }
00125             target_r0_vec.push_back(tmpr.toRotationMatrix());
00126             tm += dt;
00127         }
00128         gen_pattern_and_plot (force_diff_vec, moment_diff_vec, target_p0_vec, target_r0_vec, time_vec);
00129     };
00130     void parse_params ()
00131     {
00132       for (unsigned int i = 0; i < arg_strs.size(); ++ i) {
00133           if ( arg_strs[i]== "--use-gnuplot" ) {
00134               if (++i < arg_strs.size()) use_gnuplot = (arg_strs[i]=="true");
00135           }
00136       }
00137     };
00138 };
00139 
00140 void print_usage ()
00141 {
00142     std::cerr << "Usage : testImpedanceOutputGenerator [option]" << std::endl;
00143     std::cerr << " [option] should be:" << std::endl;
00144     std::cerr << "  --test0 : Set ref force" << std::endl;
00145     std::cerr << "  --test1 : Move pos and rot" << std::endl;
00146 };
00147 
00148 int main(int argc, char* argv[])
00149 {
00150     int ret = 0;
00151     if (argc >= 2) {
00152         testImpedanceOutputGenerator tiog;
00153         for (int i = 1; i < argc; ++ i) {
00154           tiog.arg_strs.push_back(std::string(argv[i]));
00155         }
00156         if (std::string(argv[1]) == "--test0") {
00157             tiog.test0();
00158         } else if (std::string(argv[1]) == "--test1") {
00159             tiog.test1();
00160         } else {
00161             print_usage();
00162             ret = 1;
00163         }
00164     } else {
00165         print_usage();
00166         ret = 1;
00167     }
00168     return ret;
00169 }
00170 


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