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;
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
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
00064
00065
00066
00067 int counter;
00068
00069
00070 counter=0;
00071
00072
00073 cout << "Give experiment number= line number in files \n ?";
00074 cin >> linenr;
00075
00076
00077
00078
00079
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
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
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
00109
00110 counter=0;
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
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
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
00138
00139 counter=0;
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
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
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 }