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