iotest.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 #include <kdl/error_stack.h>
00003 #include <kdl/error.h>
00004 #include <kdl/frames.hpp>
00005 #include <kdl/frames_io.hpp>
00006 #include <kdl/kinfam/joint.hpp>
00007 #include <kdl/kinfam/serialchain.hpp>
00008 #include <kdl/kinfam/kinematicfamily_io.hpp>
00009 #include <kdl/kinfam/crs450.hpp>
00010 #include <kdl/kinfam/jnt2cartpos.hpp>
00011 #include <memory>
00012 
00013 using namespace std;
00014 using namespace KDL;
00015 
00016 void test_io(KinematicFamily* kf) {
00017         // write a kf to the file tst.dat
00018         ofstream os("tst.dat");
00019         os << kf << endl;
00020         cout << kf  << endl;
00021         os.close();
00022 
00023         // read a serial chain from the file tst.dat
00024         ifstream is ("tst.dat");
00025         KinematicFamily* kf2;
00026         try {
00027           kf2 = readKinematicFamily(is);
00028           cout << kf2 << endl;
00029         } catch (Error& err) {
00030           IOTraceOutput(cerr);
00031           cout << "ERROR : " << err.Description() << endl;
00032           exit(-1);
00033         }
00034 
00035         std::vector<double> q(6);
00036         for (int i=0;i<q.size();++i) q[i] = i*0.2;
00037         Frame F1,F2;
00038         Jnt2CartPos* jnt2cartpos  = kf->createJnt2CartPos();
00039         Jnt2CartPos* jnt2cartpos2 = kf2->createJnt2CartPos();
00040 
00041         jnt2cartpos->evaluate(q);jnt2cartpos->getFrame(F1);
00042         jnt2cartpos2->evaluate(q);jnt2cartpos2->getFrame(F2);
00043         cout << F1 << endl;
00044         cout << F2 << endl;
00045         if (!Equal(F1,F2)) {
00046                 cerr << "Results are not equal" << endl;
00047                 exit(-1);
00048         }
00049         delete jnt2cartpos;
00050         delete jnt2cartpos2;
00051         delete kf2;
00052         delete kf;
00053 }
00054 
00055 int main(int argc,char* argv[]) {
00056     test_io(new CRS450());
00057     test_io(new CRS450Feath());
00058 }
00059 
00060 


orocos_kdl
Author(s):
autogenerated on Sat Oct 7 2017 03:04:28