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 #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());
00028 JntArray G(kLWR.getNrOfJoints());
00029 Wrenches f(kLWR.getNrOfSegments());
00030 Vector grav(0.0,0.0,-9.81);
00031 JntSpaceInertiaMatrix H(kLWR.getNrOfJoints());
00032 ChainDynParam chaindynparams(kLWR,grav);
00033
00034 int linenum;
00035
00036 getInputs(q, qdot,qdotdot,linenum);
00037
00038
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
00050 chaindynparams.JntToMass(q,H);
00051 chaindynparams.JntToCoriolis(q,qdot,C);
00052 chaindynparams.JntToGravity(q,G);
00053
00054
00055 Multiply(H, qdotdot, tauHCG);
00056 Add(tauHCG,C,tauHCGa);
00057 Add(tauHCGa,G,tauHCG);
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
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
00088
00089
00090
00091 int counter;
00092
00093
00094 counter=0;
00095
00096
00097 cout << "Give experiment number= line number in files \n ?";
00098 cin >> linenr;
00099
00100
00101
00102
00103
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
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
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
00133
00134 counter=0;
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
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
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
00162
00163 counter=0;
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
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
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 }