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(<rans_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
