toolkittest.cpp
Go to the documentation of this file.
1 
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>
11 
12 using namespace RTT;
13 using namespace KDL;
14 using namespace std;
15 
16 int ORO_main(int argc, char** argv)
17 {
18  RTT::Toolkit::Import(KDLToolkit);
19 
20  TaskContext writer("writer");
21  int retval = 0;
22 
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)));
29 
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);
42 
43  Property<SerialChain> kuka_prop("testchain","some chain",*(robot));
44  Property<ZXXZXZ> kuka_prop2("testchain2","some other chain",kuka);
45 
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(&ltrans_prop);
53  writer.properties()->addProperty(&utrans_prop);
54  writer.properties()->addProperty(&kuka_prop);
55  writer.properties()->addProperty(&kuka_prop2);
56 
57  writer.marshalling()->writeProperties("test.cpf");
58 
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();
64  retval=-1;
65  }
66 
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();
70  retval=-1;
71  }
72 
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();
76  retval=-1;
77  }
78 
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();
82  retval=-1;
83  }
84 
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();
88  retval=-1;
89  }
90 
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();
94  retval=-1;
95  }
96 
97 
98  delete robot;
99  return retval;
100 
101 }
102 
103 
INLINE Rall1d< T, V, S > log(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:305
int ORO_main(int argc, char **argv)
Definition: toolkittest.cpp:16
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
Definition: frameacc.hpp:394
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:160
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570


orocos_kdl
Author(s):
autogenerated on Fri Mar 12 2021 03:05:44