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


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