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