kukaLWRtestHCG.cpp
Go to the documentation of this file.
1 #include <chain.hpp>
2 #include "models.hpp"
3 #include <frames_io.hpp>
4 #include <kinfam_io.hpp> //know how to print different types on screen
5 
9 #include <chaindynparam.hpp>
10 
11 using namespace KDL;
12 using namespace std;
13 
14 void outputLine( double, double, double, double, double, double, double);
15 int getInputs(JntArray&, JntArray&, JntArray&, int&);
16 
17 int main(int argc , char** argv){
18 
19  Chain kLWR=KukaLWR_DHnew();
20 
21  JntArray q(kLWR.getNrOfJoints());
22  JntArray qdot(kLWR.getNrOfJoints());
23  JntArray qdotdot(kLWR.getNrOfJoints());
24  JntArray tau(kLWR.getNrOfJoints());
25  JntArray tauHCGa(kLWR.getNrOfJoints());
26  JntArray tauHCG(kLWR.getNrOfJoints());
27  JntArray C(kLWR.getNrOfJoints()); //coriolis matrix
28  JntArray G(kLWR.getNrOfJoints()); //gravity matrix
29  Wrenches f(kLWR.getNrOfSegments());
30  Vector grav(0.0,0.0,-9.81);
31  JntSpaceInertiaMatrix H(kLWR.getNrOfJoints()); //inertiamatrix H=square matrix of size= number of joints
32  ChainDynParam chaindynparams(kLWR,grav);
33 
34  int linenum; //number of experiment= number of line
35  //read out inputs from files
36  getInputs(q, qdot,qdotdot,linenum);
37 
38  //calculation of torques with kukaLWRDH_new.cpp (dynamic model)
39  ChainFkSolverPos_recursive fksolver(kLWR);
40  Frame T;
41  ChainIdSolver_RNE idsolver(kLWR,grav);
42 
43  fksolver.JntToCart(q,T);
44  idsolver.CartToJnt(q,qdot,qdotdot,f,tau);
45 
46  std::cout<<"pose (with dynamic model): \n"<<T<<std::endl;
47  std::cout<<"tau (with dynamic model): \n"<<tau<<std::endl;
48 
49  //calculation of the HCG matrices
50  chaindynparams.JntToMass(q,H);
51  chaindynparams.JntToCoriolis(q,qdot,C);
52  chaindynparams.JntToGravity(q,G);
53 
54  //calculation of the torques with the HCG matrices
55  Multiply(H, qdotdot, tauHCG); //H*qdotdot
56  Add(tauHCG,C,tauHCGa); //tauHCGa=H*qdotdot+C
57  Add(tauHCGa,G,tauHCG); //tauHCG=H*qdotdot+C+G
58 
59  std::cout<<"H= \n"<<H<<"\n C = \n "<<C<<"\n G= \n"<<G<<" \n tau (with HCG)= \n"<< tauHCG <<std::endl;
60 
61  //write file: code based on example 14.4, c++ how to program, Deitel and Deitel, book p 708
62  ofstream outPoseFile("poseResultaat.dat",ios::app);
63  if(!outPoseFile)
64  {
65  cerr << "File poseResultaat could not be opened" <<endl;
66  exit(1);
67  }
68  outPoseFile << "linenumber=experimentnr= "<< linenum << "\n";
69  outPoseFile << T << "\n \n";
70  outPoseFile.close();
71 
72  ofstream outTauFile("tauResultaat.dat",ios::app);
73  if(!outTauFile)
74  {
75  cerr << "File tauResultaat could not be opened" <<endl;
76  exit(1);
77  }
78  outTauFile << setiosflags( ios::left) << setw(10) << linenum;
79  outTauFile << tau << "\n";
80  outTauFile.close();
81 }
82 
83 
84 
85 int getInputs(JntArray &_q, JntArray &_qdot, JntArray &_qdotdot, int &linenr)
86 {
87  //cout << " q" << _q<< "\n";
88 
89  //declaration
90  //int linenr; //line =experiment number
91  int counter;
92 
93  //initialisation
94  counter=0;
95 
96  //ask which experiment number= line number in files
97  cout << "Give experiment number= line number in files \n ?";
98  cin >> linenr;
99 
100  //read files: code based on example 14.8, c++ how to program, Deitel and Deitel, book p 712
101 
102  /*
103  *READING Q = joint positions
104  */
105 
106  ifstream inQfile("interpreteerbaar/q", ios::in);
107 
108  if (!inQfile)
109  {
110  cerr << "File q could not be opened \n";
111  exit(1);
112  }
113 
114  //print headers
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" ;
116 
117  while(!inQfile.eof())
118  {
119  //read out a line of the file
120  inQfile >> _q(0) >> _q(1) >> _q(2) >> _q(3) >> _q(4) >> _q(5) >> _q(6);
121  counter++;
122  if(counter==linenr)
123  {
124  outputLine( _q(0), _q(1), _q(2), _q(3), _q(4), _q(5), _q(6));
125  break;
126  }
127 
128  }
129  inQfile.close();
130 
131  /*
132  *READING Qdot = joint velocities
133  */
134  counter=0;//reset counter
135  ifstream inQdotfile("interpreteerbaar/qdot", ios::in);
136 
137  if (!inQdotfile)
138  {
139  cerr << "File qdot could not be opened \n";
140  exit(1);
141  }
142 
143  //print headers
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" ;
145 
146  while(!inQdotfile.eof())
147  {
148  //read out a line of the file
149  inQdotfile >> _qdot(0) >> _qdot(1) >> _qdot(2) >> _qdot(3) >> _qdot(4) >> _qdot(5) >> _qdot(6) ;
150  counter++;
151  if(counter==linenr)
152  {
153  outputLine( _qdot(0), _qdot(1), _qdot(2), _qdot(3), _qdot(4), _qdot(5), _qdot(6));
154  break;
155  }
156 
157  }
158  inQdotfile.close();
159 
160  /*
161  *READING Qdotdot = joint accelerations
162  */
163  counter=0;//reset counter
164  ifstream inQdotdotfile("interpreteerbaar/qddot", ios::in);
165 
166  if (!inQdotdotfile)
167  {
168  cerr << "File qdotdot could not be opened \n";
169  exit(1);
170  }
171 
172  //print headers
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" ;
174 
175  while(!inQdotdotfile.eof())
176  {
177  //read out a line of the file
178  inQdotdotfile >> _qdotdot(0) >> _qdotdot(1) >> _qdotdot(2) >> _qdotdot(3) >> _qdotdot(4) >> _qdotdot(5) >> _qdotdot(6);
179  counter++;
180  if(counter==linenr)
181  {
182  outputLine(_qdotdot(0), _qdotdot(1), _qdotdot(2), _qdotdot(3), _qdotdot(4), _qdotdot(5), _qdotdot(6) );
183  break;
184  }
185 
186  }
187  inQdotdotfile.close();
188 
189 
190  return 0;
191 }
192 
193 void outputLine( double x1, double x2, double x3, double x4, double x5, double x6, double x7)
194 {
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";
197 }
Chain KukaLWR_DHnew()
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...
Definition: chain.hpp:35
unsigned int getNrOfSegments() const
Definition: chain.hpp:76
This class represents an fixed size array containing joint values of a KDL::Chain.
Definition: jntarray.hpp:69
int main(int argc, char **argv)
void Add(const JntArray &src1, const JntArray &src2, JntArray &dest)
Definition: jntarray.cpp:82
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:160
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
Definition: chain.hpp:71
int getInputs(JntArray &, JntArray &, JntArray &, int &)
std::vector< Wrench > Wrenches
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
void Multiply(const JntArray &src, const double &factor, JntArray &dest)
Definition: jntarray.cpp:92


orocos_kdl
Author(s):
autogenerated on Sat Jun 15 2019 19:07:36