2 #include"kdl/kdltk/toolkit.hpp" 3 #include<kdl/kinfam/joint_io.hpp> 4 #include<kdl/kinfam/joint.hpp> 5 #include<rtt/os/main.h> 6 #include<rtt/Logger.hpp> 7 #include<rtt/TaskContext.hpp> 8 #include<kdl/kinfam/kuka361.hpp> 9 #include<kdl/kinfam/unittransmission.hpp> 10 #include<kdl/kinfam/lineartransmission.hpp> 18 RTT::Toolkit::Import(KDLToolkit);
20 TaskContext writer(
"writer");
23 JointTransX tx(
Frame(Rotation::RPY(1,2,3),
Vector(1,2,3)));
24 JointTransY ty(
Frame(Rotation::RPY(1,2,3),
Vector(1,2,3)));
25 JointTransZ tz(
Frame(Rotation::RPY(1,2,3),
Vector(1,2,3)));
26 JointRotX rx(
Frame(Rotation::RPY(1,2,3),
Vector(1,2,3)));
27 JointRotY ry(
Frame(Rotation::RPY(1,2,3),
Vector(1,2,3)));
28 JointRotZ rz(
Frame(Rotation::RPY(1,2,3),
Vector(1,2,3)));
30 Property<JointTransX> tx_prop(
"joint1",
"first joint",tx);
31 Property<JointTransY> ty_prop(
"joint2",
"second joint",ty);
32 Property<JointTransZ> tz_prop(
"joint3",
"third joint",tz);
33 Property<JointRotX> rx_prop(
"joint4",
"fourth joint",rx);
34 Property<JointRotY> ry_prop(
"joint5",
"fifth joint",ry);
35 Property<JointRotZ> rz_prop(
"joint6",
"sixth joint",rz);
36 Kuka361 kuka = Kuka361();
37 SerialChain* robot = kuka.createSerialChain();
38 LinearTransmission ltrans = LinearTransmission(6,vector<double>(6,1),vector<double>(6,0.5));
39 Property<LinearTransmission> ltrans_prop(
"transmission",
"some transmission",ltrans);
40 UnitTransmission utrans = UnitTransmission(6);
41 Property<UnitTransmission> utrans_prop(
"transmission2",
"some other transmission",utrans);
43 Property<SerialChain> kuka_prop(
"testchain",
"some chain",*(robot));
44 Property<ZXXZXZ> kuka_prop2(
"testchain2",
"some other chain",kuka);
46 writer.properties()->addProperty(&tx_prop);
47 writer.properties()->addProperty(&ty_prop);
48 writer.properties()->addProperty(&tz_prop);
49 writer.properties()->addProperty(&rx_prop);
50 writer.properties()->addProperty(&ry_prop);
51 writer.properties()->addProperty(&rz_prop);
52 writer.properties()->addProperty(<rans_prop);
53 writer.properties()->addProperty(&utrans_prop);
54 writer.properties()->addProperty(&kuka_prop);
55 writer.properties()->addProperty(&kuka_prop2);
57 writer.marshalling()->writeProperties(
"test.cpf");
59 writer.marshalling()->readProperties(
"test.cpf");
60 Logger::In in(
"KDLToolkitTest");
61 if(tx_prop.value().getType()!=tx.getType()||!
Equal(tx_prop.value().frame_before_joint(),tx.frame_before_joint(),1e-5)){
62 log(
Error)<<
"Property is not the same after writing and rereading"<<endlog();
63 log(Debug)<<
"Property "<<tx_prop.value()<<
" ,actual type: "<<tx<<endlog();
67 if(ty_prop.value().getType()!=ty.getType()||!
Equal(ty_prop.value().frame_before_joint(),ty.frame_before_joint(),1e-5)){
68 log(
Error)<<
"Property is not the same after writing and rereading"<<endlog();
69 log(Debug)<<
"Property "<<ty_prop.value()<<
" ,actual type: "<<ty<<endlog();
73 if(tz_prop.value().getType()!=tz.getType()||!
Equal(tz_prop.value().frame_before_joint(),tz.frame_before_joint(),1e-5)){
74 log(
Error)<<
"Property is not the same after writing and rereading"<<endlog();
75 log(Debug)<<
"Property "<<tz_prop.value()<<
" ,actual type: "<<tz<<endlog();
79 if(rx_prop.value().getType()!=rx.getType()||!
Equal(rx_prop.value().frame_before_joint(),rx.frame_before_joint(),1e-5)){
80 log(
Error)<<
"Property is not the same after writing and rereading"<<endlog();
81 log(Debug)<<
"Property "<<rx_prop.value()<<
" ,actual type: "<<rx<<endlog();
85 if(ry_prop.value().getType()!=ry.getType()||!
Equal(ry_prop.value().frame_before_joint(),ry.frame_before_joint(),1e-5)){
86 log(
Error)<<
"Property is not the same after writing and rereading"<<endlog();
87 log(Debug)<<
"Property "<<ry_prop.value()<<
" ,actual type: "<<ry<<endlog();
91 if(rz_prop.value().getType()!=rz.getType()||!
Equal(rz_prop.value().frame_before_joint(),rz.frame_before_joint(),1e-5)){
92 log(
Error)<<
"Property is not the same after writing and rereading"<<endlog();
93 log(Debug)<<
"Property "<<rz_prop.value()<<
" ,actual type: "<<rz<<endlog();
INLINE Rall1d< T, V, S > log(const Rall1d< T, V, S > &arg)
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
A concrete implementation of a 3 dimensional vector class.
represents a frame transformation in 3D space (rotation + translation)