$search
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 int errorInputs = 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 }