14 void outputLine(
double,
double,
double,
double,
double,
double,
double);
17 int main(
int argc ,
char** argv){
30 Vector grav(0.0,0.0,-9.81);
46 std::cout<<
"pose (with dynamic model): \n"<<T<<std::endl;
47 std::cout<<
"tau (with dynamic model): \n"<<tau<<std::endl;
50 chaindynparams.JntToMass(q,H);
51 chaindynparams.JntToCoriolis(q,qdot,C);
52 chaindynparams.JntToGravity(q,G);
56 Add(tauHCG,C,tauHCGa);
57 Add(tauHCGa,G,tauHCG);
59 std::cout<<
"H= \n"<<H<<
"\n C = \n "<<C<<
"\n G= \n"<<G<<
" \n tau (with HCG)= \n"<< tauHCG <<std::endl;
62 ofstream outPoseFile(
"poseResultaat.dat",ios::app);
65 cerr <<
"File poseResultaat could not be opened" <<endl;
68 outPoseFile <<
"linenumber=experimentnr= "<< linenum <<
"\n";
69 outPoseFile << T <<
"\n \n";
72 ofstream outTauFile(
"tauResultaat.dat",ios::app);
75 cerr <<
"File tauResultaat could not be opened" <<endl;
78 outTauFile << setiosflags( ios::left) << setw(10) << linenum;
79 outTauFile << tau <<
"\n";
97 cout <<
"Give experiment number= line number in files \n ?";
106 ifstream inQfile(
"interpreteerbaar/q", ios::in);
110 cerr <<
"File q could not be opened \n";
115 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" ;
117 while(!inQfile.eof())
120 inQfile >> _q(0) >> _q(1) >> _q(2) >> _q(3) >> _q(4) >> _q(5) >> _q(6);
124 outputLine( _q(0), _q(1), _q(2), _q(3), _q(4), _q(5), _q(6));
135 ifstream inQdotfile(
"interpreteerbaar/qdot", ios::in);
139 cerr <<
"File qdot could not be opened \n";
144 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" ;
146 while(!inQdotfile.eof())
149 inQdotfile >> _qdot(0) >> _qdot(1) >> _qdot(2) >> _qdot(3) >> _qdot(4) >> _qdot(5) >> _qdot(6) ;
153 outputLine( _qdot(0), _qdot(1), _qdot(2), _qdot(3), _qdot(4), _qdot(5), _qdot(6));
164 ifstream inQdotdotfile(
"interpreteerbaar/qddot", ios::in);
168 cerr <<
"File qdotdot could not be opened \n";
173 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" ;
175 while(!inQdotdotfile.eof())
178 inQdotdotfile >> _qdotdot(0) >> _qdotdot(1) >> _qdotdot(2) >> _qdotdot(3) >> _qdotdot(4) >> _qdotdot(5) >> _qdotdot(6);
182 outputLine(_qdotdot(0), _qdotdot(1), _qdotdot(2), _qdotdot(3), _qdotdot(4), _qdotdot(5), _qdotdot(6) );
187 inQdotdotfile.close();
193 void outputLine(
double x1,
double x2,
double x3,
double x4,
double x5,
double x6,
double x7)
195 cout << setiosflags(ios::left) << setiosflags(ios::fixed | ios::showpoint) <<setw(15)
196 << x1 << setw(15) << x2 <<setw(15) <<setw(15) << x3 <<setw(15) << x4 <<setw(15) << x5 <<setw(15) << x6 <<setw(15) << x7 <<
"\n";
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
void outputLine(double, double, double, double, double, double, double)
This class encapsulates a serial kinematic interconnection structure. It is built out of segments...
unsigned int getNrOfSegments() const
This class represents an fixed size array containing joint values of a KDL::Chain.
int main(int argc, char **argv)
void Add(const JntArray &src1, const JntArray &src2, JntArray &dest)
A concrete implementation of a 3 dimensional vector class.
Recursive newton euler inverse dynamics solver.
int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches &f_ext, JntArray &torques)
unsigned int getNrOfJoints() const
int getInputs(JntArray &, JntArray &, JntArray &, int &)
std::vector< Wrench > Wrenches
represents a frame transformation in 3D space (rotation + translation)
void Multiply(const JntArray &src, const double &factor, JntArray &dest)