kukaLWRtestHCG.cpp
Go to the documentation of this file.
00001 #include <chain.hpp>
00002 #include "models.hpp"
00003 #include <frames_io.hpp>
00004 #include <kinfam_io.hpp> //know  how to print different types on screen
00005 
00006 #include <chainfksolverpos_recursive.hpp>
00007 #include <chainidsolver_recursive_newton_euler.hpp>
00008 #include <jntspaceinertiamatrix.hpp>
00009 #include <chaindynparam.hpp>
00010 
00011 using namespace KDL;
00012 using namespace std;
00013 
00014 void outputLine( double, double, double, double, double, double, double);
00015 int getInputs(JntArray&, JntArray&, JntArray&, int&);
00016 
00017 int main(int argc , char** argv){
00018     
00019     Chain kLWR=KukaLWR_DHnew();
00020   
00021     JntArray q(kLWR.getNrOfJoints());
00022     JntArray qdot(kLWR.getNrOfJoints());
00023     JntArray qdotdot(kLWR.getNrOfJoints());
00024     JntArray tau(kLWR.getNrOfJoints());
00025     JntArray tauHCGa(kLWR.getNrOfJoints());
00026     JntArray tauHCG(kLWR.getNrOfJoints());
00027     JntArray C(kLWR.getNrOfJoints()); //coriolis matrix
00028     JntArray G(kLWR.getNrOfJoints()); //gravity matrix
00029     Wrenches f(kLWR.getNrOfSegments());
00030     Vector grav(0.0,0.0,-9.81);
00031     JntSpaceInertiaMatrix H(kLWR.getNrOfJoints()); //inertiamatrix H=square matrix of size= number of joints
00032     ChainDynParam chaindynparams(kLWR,grav);   
00033     
00034     int linenum; //number of experiment= number of line 
00035     //read out inputs from files
00036     getInputs(q, qdot,qdotdot,linenum);
00037     
00038     //calculation of torques with kukaLWRDH_new.cpp (dynamic model)
00039     ChainFkSolverPos_recursive fksolver(kLWR);
00040     Frame T;
00041     ChainIdSolver_RNE idsolver(kLWR,grav);
00042 
00043     fksolver.JntToCart(q,T);
00044     idsolver.CartToJnt(q,qdot,qdotdot,f,tau);
00045 
00046     std::cout<<"pose (with dynamic model): \n"<<T<<std::endl;
00047     std::cout<<"tau (with dynamic model): \n"<<tau<<std::endl;
00048     
00049     //calculation of the HCG matrices
00050     chaindynparams.JntToMass(q,H);
00051     chaindynparams.JntToCoriolis(q,qdot,C);
00052     chaindynparams.JntToGravity(q,G);
00053     
00054     //calculation of the torques with the HCG matrices
00055     Multiply(H, qdotdot, tauHCG); //H*qdotdot
00056     Add(tauHCG,C,tauHCGa); //tauHCGa=H*qdotdot+C
00057     Add(tauHCGa,G,tauHCG); //tauHCG=H*qdotdot+C+G
00058         
00059     std::cout<<"H= \n"<<H<<"\n C = \n "<<C<<"\n G= \n"<<G<<" \n tau (with HCG)= \n"<< tauHCG  <<std::endl;
00060     
00061     //write file: code based on example 14.4, c++ how to program, Deitel and Deitel, book p 708
00062     ofstream outPoseFile("poseResultaat.dat",ios::app);
00063     if(!outPoseFile)
00064     {
00065     cerr << "File poseResultaat could not be opened" <<endl;
00066     exit(1);
00067     }
00068     outPoseFile << "linenumber=experimentnr= "<< linenum << "\n";
00069     outPoseFile << T << "\n \n";
00070     outPoseFile.close();
00071 
00072     ofstream outTauFile("tauResultaat.dat",ios::app);
00073     if(!outTauFile)
00074     {
00075     cerr << "File tauResultaat could not be opened" <<endl;
00076     exit(1);
00077     }
00078     outTauFile << setiosflags( ios::left) << setw(10)  << linenum;
00079     outTauFile << tau << "\n";
00080     outTauFile.close();
00081 }
00082     
00083 
00084 
00085 int getInputs(JntArray &_q, JntArray &_qdot, JntArray &_qdotdot, int &linenr)
00086 {
00087   //cout << " q" << _q<< "\n";
00088   
00089   //declaration
00090   //int linenr; //line =experiment number
00091   int counter;
00092   
00093   //initialisation
00094   counter=0;
00095   
00096   //ask which experiment number= line number in files
00097   cout << "Give experiment number= line number in files \n ?";
00098   cin >> linenr;
00099     
00100   //read files: code based on example 14.8, c++ how to program, Deitel and Deitel, book p 712
00101   
00102   /*
00103    *READING Q = joint positions
00104    */
00105   
00106   ifstream inQfile("interpreteerbaar/q", ios::in);
00107 
00108   if (!inQfile)
00109   {
00110     cerr << "File q could not be opened \n";
00111     exit(1);
00112   }
00113   
00114   //print headers
00115   cout << setiosflags( ios::left) << setw(15) << "_q(0)" << setw(15) << "_q(1)" << setw(15) << "_q(2)" << setw(15) << "_q(3)" << setw(15) << "_q(4)" << setw(15) << "_q(5)" << setw(15) << "_q(6)"   << " \n" ;
00116   
00117   while(!inQfile.eof())
00118   {
00119     //read out a line of the file
00120     inQfile >> _q(0) >> _q(1) >> _q(2) >> _q(3) >> _q(4) >> _q(5) >> _q(6); 
00121     counter++;
00122     if(counter==linenr)
00123     {
00124       outputLine( _q(0), _q(1), _q(2), _q(3), _q(4), _q(5), _q(6));
00125       break;
00126     }
00127     
00128   }
00129   inQfile.close();
00130   
00131   /*
00132    *READING Qdot = joint velocities
00133    */
00134   counter=0;//reset counter
00135   ifstream inQdotfile("interpreteerbaar/qdot", ios::in);
00136 
00137   if (!inQdotfile)
00138   {
00139     cerr << "File qdot could not be opened \n";
00140     exit(1);
00141   }
00142   
00143   //print headers
00144   cout << setiosflags( ios::left) << setw(15) << "_qdot(0)" << setw(15) << "_qdot(1)" << setw(15) << "_qdot(2)" << setw(15) << "_qdot(3)" << setw(15) << "_qdot(4)" << setw(15) << "_qdot(5)" << setw(15) << "_qdot(6)"   << " \n" ;
00145   
00146   while(!inQdotfile.eof())
00147   {
00148     //read out a line of the file
00149     inQdotfile >> _qdot(0) >> _qdot(1) >> _qdot(2) >> _qdot(3) >> _qdot(4) >> _qdot(5) >> _qdot(6) ; 
00150     counter++;
00151     if(counter==linenr)
00152     {
00153       outputLine( _qdot(0), _qdot(1), _qdot(2), _qdot(3), _qdot(4), _qdot(5), _qdot(6));
00154       break;
00155     }
00156     
00157   }
00158   inQdotfile.close(); 
00159   
00160   /*
00161    *READING Qdotdot = joint accelerations
00162    */
00163   counter=0;//reset counter
00164   ifstream inQdotdotfile("interpreteerbaar/qddot", ios::in);
00165 
00166   if (!inQdotdotfile)
00167   {
00168     cerr << "File qdotdot could not be opened \n";
00169     exit(1);
00170   }
00171   
00172   //print headers
00173   cout << setiosflags( ios::left) << setw(15) << "_qdotdot(0)" << setw(15) << "_qdotdot(1)" << setw(15) << "_qdotdot(2)" << setw(15) << "_qdotdot(3)" << setw(15) << "_qdotdot(4)" << setw(15) << "_qdotdot(5)" << setw(15) << "_qdotdot(6)"  << " \n" ;
00174   
00175   while(!inQdotdotfile.eof())
00176   {
00177     //read out a line of the file
00178     inQdotdotfile >> _qdotdot(0) >> _qdotdot(1) >> _qdotdot(2) >> _qdotdot(3) >> _qdotdot(4) >> _qdotdot(5) >> _qdotdot(6); 
00179     counter++;
00180     if(counter==linenr)
00181     {
00182       outputLine(_qdotdot(0), _qdotdot(1), _qdotdot(2), _qdotdot(3), _qdotdot(4), _qdotdot(5), _qdotdot(6) );
00183       break;
00184     }
00185     
00186   }
00187   inQdotdotfile.close();
00188 
00189   
00190   return 0;
00191 }
00192 
00193 void outputLine( double x1, double x2, double x3, double x4, double x5, double x6, double x7)
00194 {
00195   cout << setiosflags(ios::left) << setiosflags(ios::fixed | ios::showpoint) <<setw(15) 
00196   << x1 << setw(15) << x2 <<setw(15) <<setw(15) << x3 <<setw(15) << x4 <<setw(15) << x5 <<setw(15) << x6 <<setw(15) << x7 <<"\n";
00197 }


orocos_kdl
Author(s):
autogenerated on Wed Aug 26 2015 15:14:14