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