iotest.cpp
Go to the documentation of this file.
1 #include <iostream>
2 #include <kdl/error_stack.h>
3 #include <kdl/error.h>
4 #include <kdl/frames.hpp>
5 #include <kdl/frames_io.hpp>
6 #include <kdl/kinfam/joint.hpp>
7 #include <kdl/kinfam/serialchain.hpp>
8 #include <kdl/kinfam/kinematicfamily_io.hpp>
9 #include <kdl/kinfam/crs450.hpp>
10 #include <kdl/kinfam/jnt2cartpos.hpp>
11 #include <memory>
12 
13 using namespace std;
14 using namespace KDL;
15 
16 void test_io(KinematicFamily* kf) {
17  // write a kf to the file tst.dat
18  ofstream os("tst.dat");
19  os << kf << endl;
20  cout << kf << endl;
21  os.close();
22 
23  // read a serial chain from the file tst.dat
24  ifstream is ("tst.dat");
25  KinematicFamily* kf2;
26  try {
27  kf2 = readKinematicFamily(is);
28  cout << kf2 << endl;
29  } catch (Error& err) {
30  IOTraceOutput(cerr);
31  cout << "ERROR : " << err.Description() << endl;
32  exit(-1);
33  }
34 
35  std::vector<double> q(6);
36  for (int i=0;i<q.size();++i) q[i] = i*0.2;
37  Frame F1,F2;
38  Jnt2CartPos* jnt2cartpos = kf->createJnt2CartPos();
39  Jnt2CartPos* jnt2cartpos2 = kf2->createJnt2CartPos();
40 
41  jnt2cartpos->evaluate(q);jnt2cartpos->getFrame(F1);
42  jnt2cartpos2->evaluate(q);jnt2cartpos2->getFrame(F2);
43  cout << F1 << endl;
44  cout << F2 << endl;
45  if (!Equal(F1,F2)) {
46  cerr << "Results are not equal" << endl;
47  exit(-1);
48  }
49  delete jnt2cartpos;
50  delete jnt2cartpos2;
51  delete kf2;
52  delete kf;
53 }
54 
55 int main(int argc,char* argv[]) {
56  test_io(new CRS450());
57  test_io(new CRS450Feath());
58 }
59 
60 
void test_io(KinematicFamily *kf)
Definition: iotest.cpp:16
void IOTraceOutput(std::ostream &os)
outputs the IO-stack to a stream to provide a better errormessage.
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
Definition: frameacc.hpp:394
int main(int argc, char *argv[])
Definition: iotest.cpp:55
virtual const char * Description() const
Definition: error.h:63
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570


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