orocos_kdl
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 KDL;
14 
15 void test_io(KinematicFamily* kf) {
16  // write a kf to the file tst.dat
17  std::ofstream os("tst.dat");
18  os << kf << std::endl;
19  std::cout << kf << std::endl;
20  os.close();
21 
22  // read a serial chain from the file tst.dat
23  std::ifstream is ("tst.dat");
24  KinematicFamily* kf2;
25  try {
26  kf2 = readKinematicFamily(is);
27  std::cout << kf2 << std::endl;
28  } catch (Error& err) {
30  std::cout << "ERROR : " << err.Description() << std::endl;
31  exit(-1);
32  }
33 
35  for (int i=0;i<q.size();++i) q[i] = i*0.2;
36  Frame F1,F2;
37  Jnt2CartPos* jnt2cartpos = kf->createJnt2CartPos();
38  Jnt2CartPos* jnt2cartpos2 = kf2->createJnt2CartPos();
39 
40  jnt2cartpos->evaluate(q);jnt2cartpos->getFrame(F1);
41  jnt2cartpos2->evaluate(q);jnt2cartpos2->getFrame(F2);
42  std::cout << F1 << std::endl;
43  std::cout << F2 << std::endl;
44  if (!Equal(F1,F2)) {
45  std::cerr << "Results are not equal" << std::endl;
46  exit(-1);
47  }
48  delete jnt2cartpos;
49  delete jnt2cartpos2;
50  delete kf2;
51  delete kf;
52 }
53 
54 int main(int argc,char* argv[]) {
55  test_io(new CRS450());
56  test_io(new CRS450Feath());
57 }
58 
59 
KDL::Error::Description
virtual const char * Description() const
Definition: error.h:143
std::vector< double >
std::vector::size
T size(T... args)
std::cerr
test_io
void test_io(KinematicFamily *kf)
Definition: iotest.cpp:15
iostream
KDL
Definition: kukaLWR_DHnew.cpp:25
std::cout
std::ofstream
main
int main(int argc, char *argv[])
Definition: iotest.cpp:54
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
std::ofstream::close
T close(T... args)
KDL::Error
Definition: error.h:96
memory
std::endl
T endl(T... args)
KDL::Equal
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
KDL::IOTraceOutput
void IOTraceOutput(std::ostream &os)
outputs the IO-stack to a stream to provide a better errormessage.
Definition: error_stack.cxx:55
std::ifstream