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
00018 ofstream os("tst.dat");
00019 os << kf << endl;
00020 cout << kf << endl;
00021 os.close();
00022
00023
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