toolkittest.cpp
Go to the documentation of this file.
00001 
00002 #include"kdl/kdltk/toolkit.hpp"
00003 #include<kdl/kinfam/joint_io.hpp>
00004 #include<kdl/kinfam/joint.hpp>
00005 #include<rtt/os/main.h>
00006 #include<rtt/Logger.hpp>
00007 #include<rtt/TaskContext.hpp>
00008 #include<kdl/kinfam/kuka361.hpp>
00009 #include<kdl/kinfam/unittransmission.hpp>
00010 #include<kdl/kinfam/lineartransmission.hpp>
00011 
00012 using namespace RTT;
00013 using namespace KDL;
00014 using namespace std;
00015 
00016 int ORO_main(int argc, char** argv)
00017 {
00018     RTT::Toolkit::Import(KDLToolkit);
00019 
00020     TaskContext writer("writer");
00021     int retval = 0;
00022 
00023     JointTransX tx(Frame(Rotation::RPY(1,2,3),Vector(1,2,3)));
00024     JointTransY ty(Frame(Rotation::RPY(1,2,3),Vector(1,2,3)));
00025     JointTransZ tz(Frame(Rotation::RPY(1,2,3),Vector(1,2,3)));
00026     JointRotX rx(Frame(Rotation::RPY(1,2,3),Vector(1,2,3)));
00027     JointRotY ry(Frame(Rotation::RPY(1,2,3),Vector(1,2,3)));
00028     JointRotZ rz(Frame(Rotation::RPY(1,2,3),Vector(1,2,3)));
00029 
00030     Property<JointTransX> tx_prop("joint1","first joint",tx);
00031     Property<JointTransY> ty_prop("joint2","second joint",ty);
00032     Property<JointTransZ> tz_prop("joint3","third joint",tz);
00033     Property<JointRotX> rx_prop("joint4","fourth joint",rx);
00034     Property<JointRotY> ry_prop("joint5","fifth joint",ry);
00035     Property<JointRotZ> rz_prop("joint6","sixth joint",rz);
00036     Kuka361 kuka = Kuka361();
00037     SerialChain* robot = kuka.createSerialChain();
00038     LinearTransmission ltrans = LinearTransmission(6,vector<double>(6,1),vector<double>(6,0.5));
00039     Property<LinearTransmission> ltrans_prop("transmission","some transmission",ltrans);
00040     UnitTransmission utrans = UnitTransmission(6);
00041     Property<UnitTransmission> utrans_prop("transmission2","some other transmission",utrans);
00042 
00043     Property<SerialChain> kuka_prop("testchain","some chain",*(robot));
00044     Property<ZXXZXZ> kuka_prop2("testchain2","some other chain",kuka);
00045 
00046     writer.properties()->addProperty(&tx_prop);
00047     writer.properties()->addProperty(&ty_prop);
00048     writer.properties()->addProperty(&tz_prop);
00049     writer.properties()->addProperty(&rx_prop);
00050     writer.properties()->addProperty(&ry_prop);
00051     writer.properties()->addProperty(&rz_prop);
00052     writer.properties()->addProperty(&ltrans_prop);
00053     writer.properties()->addProperty(&utrans_prop);
00054     writer.properties()->addProperty(&kuka_prop);
00055     writer.properties()->addProperty(&kuka_prop2);
00056 
00057     writer.marshalling()->writeProperties("test.cpf");
00058 
00059     writer.marshalling()->readProperties("test.cpf");
00060     Logger::In in("KDLToolkitTest");
00061     if(tx_prop.value().getType()!=tx.getType()||!Equal(tx_prop.value().frame_before_joint(),tx.frame_before_joint(),1e-5)){
00062         log(Error)<<"Property is not the same after writing and rereading"<<endlog();
00063         log(Debug)<<"Property "<<tx_prop.value()<<" ,actual type: "<<tx<<endlog();
00064         retval=-1;
00065     }
00066 
00067     if(ty_prop.value().getType()!=ty.getType()||!Equal(ty_prop.value().frame_before_joint(),ty.frame_before_joint(),1e-5)){
00068         log(Error)<<"Property is not the same after writing and rereading"<<endlog();
00069         log(Debug)<<"Property "<<ty_prop.value()<<" ,actual type: "<<ty<<endlog();
00070         retval=-1;
00071     }
00072 
00073     if(tz_prop.value().getType()!=tz.getType()||!Equal(tz_prop.value().frame_before_joint(),tz.frame_before_joint(),1e-5)){
00074         log(Error)<<"Property is not the same after writing and rereading"<<endlog();
00075         log(Debug)<<"Property "<<tz_prop.value()<<" ,actual type: "<<tz<<endlog();
00076         retval=-1;
00077     }
00078 
00079     if(rx_prop.value().getType()!=rx.getType()||!Equal(rx_prop.value().frame_before_joint(),rx.frame_before_joint(),1e-5)){
00080         log(Error)<<"Property is not the same after writing and rereading"<<endlog();
00081         log(Debug)<<"Property "<<rx_prop.value()<<" ,actual type: "<<rx<<endlog();
00082         retval=-1;
00083     }
00084 
00085     if(ry_prop.value().getType()!=ry.getType()||!Equal(ry_prop.value().frame_before_joint(),ry.frame_before_joint(),1e-5)){
00086         log(Error)<<"Property is not the same after writing and rereading"<<endlog();
00087         log(Debug)<<"Property "<<ry_prop.value()<<" ,actual type: "<<ry<<endlog();
00088         retval=-1;
00089     }
00090 
00091     if(rz_prop.value().getType()!=rz.getType()||!Equal(rz_prop.value().frame_before_joint(),rz.frame_before_joint(),1e-5)){
00092         log(Error)<<"Property is not the same after writing and rereading"<<endlog();
00093         log(Debug)<<"Property "<<rz_prop.value()<<" ,actual type: "<<rz<<endlog();
00094         retval=-1;
00095     }
00096 
00097 
00098     delete robot;
00099     return retval;
00100 
00101 }
00102 
00103 


orocos_kdl
Author(s):
autogenerated on Wed Aug 26 2015 15:14:14