kukaLWRtestDHnew.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>
5 
8 
9 using namespace KDL;
10 using namespace std;
11 
12 void outputLine( double, double, double, double, double, double, double);
13 int getInputs(JntArray&, JntArray&, JntArray&, int&);
14 
15 int main(int argc , char** argv){
16 
17  Chain kLWR=KukaLWR_DHnew();
18 
19  JntArray q(kLWR.getNrOfJoints());
20  JntArray qdot(kLWR.getNrOfJoints());
21  JntArray qdotdot(kLWR.getNrOfJoints());
22  JntArray tau(kLWR.getNrOfJoints());
23  Wrenches f(kLWR.getNrOfSegments());
24  int linenum; //number of experiment= number of line
25  getInputs(q, qdot,qdotdot,linenum);
26 
27  ChainFkSolverPos_recursive fksolver(kLWR);
28  Frame T;
29  ChainIdSolver_RNE idsolver(kLWR,Vector(0.0,0.0,-9.81));
30 
31  fksolver.JntToCart(q,T);
32  idsolver.CartToJnt(q,qdot,qdotdot,f,tau);
33 
34  std::cout<<"pose: \n"<<T<<std::endl;
35  std::cout<<"tau: "<<tau<<std::endl;
36 
37 //write file: code based on example 14.4, c++ how to program, Deitel and Deitel, book p 708
38  ofstream outPoseFile("poseResultaat.dat",ios::app);
39  if(!outPoseFile)
40  {
41  cerr << "File poseResultaat could not be opened" <<endl;
42  exit(1);
43  }
44  outPoseFile << "linenumber=experimentnr= "<< linenum << "\n";
45  outPoseFile << T << "\n \n";
46  outPoseFile.close();
47 
48  ofstream outTauFile("tauResultaat.dat",ios::app);
49  if(!outTauFile)
50  {
51  cerr << "File tauResultaat could not be opened" <<endl;
52  exit(1);
53  }
54  outTauFile << setiosflags( ios::left) << setw(10) << linenum;
55  outTauFile << tau << "\n";
56  outTauFile.close();
57 }
58 
59 
60 
61 int getInputs(JntArray &_q, JntArray &_qdot, JntArray &_qdotdot, int &linenr)
62 {
63  //cout << " q" << _q<< "\n";
64 
65  //declaration
66  //int linenr; //line =experiment number
67  int counter;
68 
69  //initialisation
70  counter=0;
71 
72  //ask which experiment number= line number in files
73  cout << "Give experiment number= line number in files \n ?";
74  cin >> linenr;
75 
76  //read files: code based on example 14.8, c++ how to program, Deitel and Deitel, book p 712
77 
78  /*
79  *READING Q = joint positions
80  */
81 
82  ifstream inQfile("interpreteerbaar/q", ios::in);
83 
84  if (!inQfile)
85  {
86  cerr << "File q could not be opened \n";
87  exit(1);
88  }
89 
90  //print headers
91  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" ;
92 
93  while(!inQfile.eof())
94  {
95  //read out a line of the file
96  inQfile >> _q(0) >> _q(1) >> _q(2) >> _q(3) >> _q(4) >> _q(5) >> _q(6);
97  counter++;
98  if(counter==linenr)
99  {
100  outputLine( _q(0), _q(1), _q(2), _q(3), _q(4), _q(5), _q(6));
101  break;
102  }
103 
104  }
105  inQfile.close();
106 
107  /*
108  *READING Qdot = joint velocities
109  */
110  counter=0;//reset counter
111  ifstream inQdotfile("interpreteerbaar/qdot", ios::in);
112 
113  if (!inQdotfile)
114  {
115  cerr << "File qdot could not be opened \n";
116  exit(1);
117  }
118 
119  //print headers
120  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" ;
121 
122  while(!inQdotfile.eof())
123  {
124  //read out a line of the file
125  inQdotfile >> _qdot(0) >> _qdot(1) >> _qdot(2) >> _qdot(3) >> _qdot(4) >> _qdot(5) >> _qdot(6) ;
126  counter++;
127  if(counter==linenr)
128  {
129  outputLine( _qdot(0), _qdot(1), _qdot(2), _qdot(3), _qdot(4), _qdot(5), _qdot(6));
130  break;
131  }
132 
133  }
134  inQdotfile.close();
135 
136  /*
137  *READING Qdotdot = joint accelerations
138  */
139  counter=0;//reset counter
140  ifstream inQdotdotfile("interpreteerbaar/qddot", ios::in);
141 
142  if (!inQdotdotfile)
143  {
144  cerr << "File qdotdot could not be opened \n";
145  exit(1);
146  }
147 
148  //print headers
149  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" ;
150 
151  while(!inQdotdotfile.eof())
152  {
153  //read out a line of the file
154  inQdotdotfile >> _qdotdot(0) >> _qdotdot(1) >> _qdotdot(2) >> _qdotdot(3) >> _qdotdot(4) >> _qdotdot(5) >> _qdotdot(6);
155  counter++;
156  if(counter==linenr)
157  {
158  outputLine(_qdotdot(0), _qdotdot(1), _qdotdot(2), _qdotdot(3), _qdotdot(4), _qdotdot(5), _qdotdot(6) );
159  break;
160  }
161 
162  }
163  inQdotdotfile.close();
164 
165 
166  return 0;
167 }
168 
169 void outputLine( double x1, double x2, double x3, double x4, double x5, double x6, double x7)
170 {
171  cout << setiosflags(ios::left) << setiosflags(ios::fixed | ios::showpoint) <<setw(15)
172  << x1 << setw(15) << x2 <<setw(15) <<setw(15) << x3 <<setw(15) << x4 <<setw(15) << x5 <<setw(15) << x6 <<setw(15) << x7 <<"\n";
173 }
Chain KukaLWR_DHnew()
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
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 getInputs(JntArray &, JntArray &, JntArray &, int &)
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
std::vector< Wrench > Wrenches
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
void outputLine(double, double, double, double, double, double, double)
int main(int argc, char **argv)


orocos_kdl
Author(s):
autogenerated on Fri Mar 12 2021 03:05:44