rframestest.cpp
Go to the documentation of this file.
1 #include <kdl/framevel.hpp>
2 #include <kdl/frameacc.hpp>
3 #include <kdl/framevel_io.hpp>
4 #include <kdl/frameacc_io.hpp>
5 #include <kdl/test_macros.h>
6 
7 
8 using namespace KDL;
9 
10 void TestVector() {
11  KDL_CTX;
12  Vector v(3,4,5);
13  Vector v2;
14  Vector vt(-5,-6,-3);
15  KDL_DIFF( 2*v-v, v );
16  KDL_DIFF( v*2-v, v );
17  KDL_DIFF( v+v+v-2*v, v );
18  v2=v;
19  KDL_DIFF( v, v2 );
20  v2+=v;
21  KDL_DIFF( 2*v, v2 );
22  v2-=v;
23  KDL_DIFF( v,v2);
24  v2.ReverseSign();
25  KDL_DIFF( v,-v2);
26  KDL_DIFF( v*vt,-vt*v);
27  v2 = Vector(-5,-6,-3);
28  KDL_DIFF( v*v2,-v2*v);
29 }
30 
31 void TestVectorVel() {
32  KDL_CTX;
33  VectorVel v(Vector(3,-4,5),Vector(6,3,-5));
34  VectorVel v2;
35  Vector vt(-4,-6,-8);
36  KDL_DIFF( 2*v-v, v );
37  KDL_DIFF( v*2-v, v );
38  KDL_DIFF( v+v+v-2*v, v );
39  v2=v;
40  KDL_DIFF( v, v2 );
41  v2+=v;
42  KDL_DIFF( 2*v, v2 );
43  v2-=v;
44  KDL_DIFF( v,v2);
45  v2.ReverseSign();
46  KDL_DIFF( v,-v2);
47  KDL_DIFF( v*vt,-vt*v);
48  v2 = VectorVel(Vector(-5,-6,-3),Vector(3,4,5));
49  KDL_DIFF( v*v2,-v2*v);
50 }
51 
52 void TestVectorAcc() {
53  KDL_CTX;
54  VectorAcc v(Vector(3,-4,5),Vector(6,3,-5),Vector(-4,-3,-6));
55  VectorAcc v2;
56  Vector vt(-4,-6,-8);
57  KDL_DIFF( 2*v-v, v );
58  KDL_DIFF( v*2-v, v );
59  KDL_DIFF( v+v+v-2*v, v );
60  v2=v;
61  KDL_DIFF( v, v2 );
62  v2+=v;
63  KDL_DIFF( 2*v, v2 );
64  v2-=v;
65  KDL_DIFF( v,v2);
66  v2.ReverseSign();
67  KDL_DIFF( v,-v2);
68  KDL_DIFF( v*vt,-vt*v);
69  v2 = VectorAcc(Vector(-5,-6,-3),Vector(-3,-4,-1),Vector(10,12,9));
70  KDL_DIFF( v*v2,-v2*v);
71 }
72 
73 void TestRotation() {
74  KDL_CTX;
75  Vector v(Vector(9,4,-2));
76  Vector v2;
77  Vector vt(2,3,4);
78  Rotation R;
79  Rotation R2;
80  double a;
81  double b;
82  double c;
83  a= -15*deg2rad;
84  b= 20*deg2rad;
85  c= -80*deg2rad;
86  R = Rotation(Rotation::RPY(a,b,c));
87  R2=R;
88  KDL_DIFF( R,R2 );
89  KDL_DIFF( (R*v).Norm(), v.Norm() );
90  KDL_DIFF( R.Inverse(R*v), v );
91  KDL_DIFF( R*R.Inverse(v), v );
92  KDL_DIFF( R*Rotation::Identity(), R );
93  KDL_DIFF( Rotation::Identity()*R, R );
94  KDL_DIFF( R*(R*(R*v)), (R*R*R)*v );
95  KDL_DIFF( R*(R*(R*vt)), (R*R*R)*vt );
96  KDL_DIFF( R*R.Inverse(), Rotation::Identity() );
97  KDL_DIFF( R.Inverse()*R, Rotation::Identity() );
98  KDL_DIFF( R.Inverse()*v, R.Inverse(v) );
99  v2=v*v-2*v;
100  KDL_DIFF( (v2).Norm(), ::sqrt(dot(v2,v2)) );
101 }
102 
103 
104 void TestRotationVel() {
105  KDL_CTX;
106  VectorVel v(Vector(9,4,-2),Vector(-5,6,-2));
107  VectorVel v2;
108  Vector vt(2,3,4);
109  RotationVel R;
110  RotationVel R2;
111  double a;
112  double b;
113  double c;
114  a= -15*deg2rad;
115  b= 20*deg2rad;
116  c= -80*deg2rad;
117  R = RotationVel(Rotation::RPY(a,b,c),Vector(2,4,1));
118  R2=R;
119  KDL_DIFF( R,R2 );
120  KDL_DIFF( (R*v).Norm(), v.Norm() );
121  KDL_DIFF( R.Inverse(R*v), v );
122  KDL_DIFF( R*R.Inverse(v), v );
123  KDL_DIFF( R*Rotation::Identity(), R );
124  KDL_DIFF( Rotation::Identity()*R, R );
125  KDL_DIFF( R*(R*(R*v)), (R*R*R)*v );
126  KDL_DIFF( R*(R*(R*vt)), (R*R*R)*vt );
127  KDL_DIFF( R*R.Inverse(), RotationVel::Identity() );
128  KDL_DIFF( R.Inverse()*R, RotationVel::Identity() );
129  KDL_DIFF( R.Inverse()*v, R.Inverse(v) );
130  v2=v*v-2*v;
131  KDL_DIFF( (v2).Norm(), sqrt(dot(v2,v2)) );
132 }
133 
134 void TestRotationAcc() {
135  KDL_CTX;
136  VectorAcc v(Vector(9,4,-2),Vector(-5,6,-2),Vector(2,-3,-3));
137  VectorAcc v2;
138  Vector vt(2,3,4);
139  RotationAcc R;
140  RotationAcc R2;
141  double a;
142  double b;
143  double c;
144  a= -15*deg2rad;
145  b= 20*deg2rad;
146  c= -80*deg2rad;
147  R = RotationAcc(Rotation::RPY(a,b,c),Vector(2,4,1),Vector(-3,-2,-1));
148  R2=R;
149  KDL_DIFF( R,R2 );
150  KDL_DIFF( (R*v).Norm(), v.Norm() );
151  KDL_DIFF( R.Inverse(R*v), v );
152  KDL_DIFF( R*R.Inverse(v), v );
153  KDL_DIFF( R*Rotation::Identity(), R );
154  KDL_DIFF( Rotation::Identity()*R, R );
155  KDL_DIFF( R*(R*(R*v)), (R*R*R)*v );
156  KDL_DIFF( R*(R*(R*vt)), (R*R*R)*vt );
157  KDL_DIFF( R*R.Inverse(), RotationAcc::Identity() );
158  KDL_DIFF( R.Inverse()*R, RotationAcc::Identity() );
159  KDL_DIFF( R.Inverse()*v, R.Inverse(v) );
160  v2=v*v-2*v;
161  KDL_DIFF( (v2).Norm(), sqrt(dot(v2,v2)) );
162 }
163 
164 void TestFrame() {
165  KDL_CTX;
166  Vector v(3,4,5);
167  Vector vt(-1,0,-10);
168  Rotation R;
169  Frame F;
170  Frame F2;
171  F = Frame(Rotation::EulerZYX(10*deg2rad,20*deg2rad,-10*deg2rad),Vector(4,-2,1));
172 
173  F2=F;
174  KDL_DIFF( F, F2 );
175  KDL_DIFF( F.Inverse(F*v), v );
176  KDL_DIFF( F.Inverse(F*vt), vt );
177  KDL_DIFF( F*F.Inverse(v), v );
178  KDL_DIFF( F*F.Inverse(vt), vt );
179  KDL_DIFF( F*Frame::Identity(), F );
180  KDL_DIFF( Frame::Identity()*F, F );
181  KDL_DIFF( F*(F*(F*v)), (F*F*F)*v );
182  KDL_DIFF( F*(F*(F*vt)), (F*F*F)*vt );
183  KDL_DIFF( F*F.Inverse(), Frame::Identity() );
184  KDL_DIFF( F.Inverse()*F, Frame::Identity() );
185  KDL_DIFF( F.Inverse()*vt, F.Inverse(vt) );
186 }
187 
188 void TestFrameVel() {
189  KDL_CTX;
190  VectorVel v(Vector(3,4,5),Vector(-2,-4,-1));
191  Vector vt(-1,0,-10);
192  RotationVel R;
193  FrameVel F;
194  FrameVel F2; ;
195  F = FrameVel(
196  Frame(Rotation::EulerZYX(10*deg2rad,20*deg2rad,-10*deg2rad),Vector(4,-2,1)),
197  Twist(Vector(2,-2,-2),Vector(-5,-3,-2))
198  );
199  F2=F;
200  KDL_DIFF( F, F2 );
201  KDL_DIFF( F.Inverse(F*v), v );
202  KDL_DIFF( F.Inverse(F*vt), vt );
203  KDL_DIFF( F*F.Inverse(v), v );
204  KDL_DIFF( F*F.Inverse(vt), vt );
205  KDL_DIFF( F*Frame::Identity(), F );
206  KDL_DIFF( Frame::Identity()*F, F );
207  KDL_DIFF( F*(F*(F*v)), (F*F*F)*v );
208  KDL_DIFF( F*(F*(F*vt)), (F*F*F)*vt );
209  KDL_DIFF( F*F.Inverse(), FrameVel::Identity() );
210  KDL_DIFF( F.Inverse()*F, Frame::Identity() );
211  KDL_DIFF( F.Inverse()*vt, F.Inverse(vt) );
212 }
213 
214 void TestFrameAcc() {
215  KDL_CTX;
216  VectorAcc v(Vector(3,4,5),Vector(-2,-4,-1),Vector(6,4,2));
217  Vector vt(-1,0,-10);
218  RotationAcc R;
219  FrameAcc F;
220  FrameAcc F2;
221  F = FrameAcc(
222  Frame(Rotation::EulerZYX(10*deg2rad,20*deg2rad,-10*deg2rad),Vector(4,-2,1)),
223  Twist(Vector(2,-2,-2),Vector(-5,-3,-2)),
224  Twist(Vector(5,6,2),Vector(-2,-3,1))
225  );
226  F2=F;
227  KDL_DIFF( F, F2 );
228  KDL_DIFF( F.Inverse(F*v), v );
229  KDL_DIFF( F.Inverse(F*vt), vt );
230  KDL_DIFF( F*F.Inverse(v), v );
231  KDL_DIFF( F*F.Inverse(vt), vt );
232  KDL_DIFF( F*Frame::Identity(), F );
233  KDL_DIFF( Frame::Identity()*F, F );
234  KDL_DIFF( F*(F*(F*v)), (F*F*F)*v );
235  KDL_DIFF( F*(F*(F*vt)), (F*F*F)*vt );
236  KDL_DIFF( F*F.Inverse(), FrameAcc::Identity() );
237  KDL_DIFF( F.Inverse()*F, Frame::Identity() );
238  KDL_DIFF( F.Inverse()*vt, F.Inverse(vt) );
239 }
240 
241 void TestTwistVel() {
242  KDL_CTX;
243  // Twist
244  TwistVel t(VectorVel(
245  Vector(6,3,5),
246  Vector(1,4,2)
247  ),VectorVel(
248  Vector(4,-2,7),
249  Vector(-1,-2,-3)
250  )
251  );
252  TwistVel t2;
253  RotationVel R(Rotation::RPY(10*deg2rad,20*deg2rad,-15*deg2rad),Vector(-1,5,3));
254  FrameVel F = FrameVel(
255  Frame(
256  Rotation::EulerZYX(-17*deg2rad,13*deg2rad,-16*deg2rad),
257  Vector(4,-2,1)
258  ),
259  Twist(
260  Vector(2,-2,-2),
261  Vector(-5,-3,-2)
262  )
263  );
264 
265  KDL_DIFF(2.0*t-t,t);
266  KDL_DIFF(t*2.0-t,t);
267  KDL_DIFF(t+t+t-2.0*t,t);
268  t2=t;
269  KDL_DIFF(t,t2);
270  t2+=t;
271  KDL_DIFF(2.0*t,t2);
272  t2-=t;
273  KDL_DIFF(t,t2);
274  t.ReverseSign();
275  KDL_DIFF(t,-t2);
276  KDL_DIFF(R.Inverse(R*t),t);
277  KDL_DIFF(R*t,R*R.Inverse(R*t));
278  KDL_DIFF(F.Inverse(F*t),t);
279  KDL_DIFF(F*t,F*F.Inverse(F*t));
280  KDL_DIFF(doubleVel(3.14,2)*t,t*doubleVel(3.14,2));
281  KDL_DIFF(t/doubleVel(3.14,2),t*(1.0/doubleVel(3.14,2)));
282  KDL_DIFF(t/3.14,t*(1.0/3.14));
283  KDL_DIFF(-t,-1.0*t);
284  VectorVel p1(Vector(5,1,2),Vector(4,2,1)) ;
285  VectorVel p2(Vector(2,0,5),Vector(-2,7,-1)) ;
286  KDL_DIFF(t.RefPoint(p1+p2),t.RefPoint(p1).RefPoint(p2));
287  KDL_DIFF(t,t.RefPoint(-p1).RefPoint(p1));
288 }
289 
290 void TestTwistAcc() {
291  KDL_CTX;
292  // Twist
293  TwistAcc t( VectorAcc(Vector(6,3,5),Vector(1,4,2),Vector(5,2,1)),
294  VectorAcc(Vector(4,-2,7),Vector(-1,-2,-3),Vector(5,2,9) )
295  );
296  TwistAcc t2;
297  RotationAcc R(Rotation::RPY(10*deg2rad,20*deg2rad,-15*deg2rad),
298  Vector(-1,5,3),
299  Vector(2,1,3)
300  ) ;
301  FrameAcc F = FrameAcc(
302  Frame(Rotation::EulerZYX(-17*deg2rad,13*deg2rad,-16*deg2rad),Vector(4,-2,1)),
303  Twist(Vector(2,-2,-2),Vector(-5,-3,-2)),
304  Twist(Vector(5,4,-5),Vector(12,13,17))
305  );
306 
307  KDL_DIFF(2.0*t-t,t);
308  KDL_DIFF(t*2.0-t,t);
309  KDL_DIFF(t+t+t-2.0*t,t);
310  t2=t;
311  KDL_DIFF(t,t2);
312  t2+=t;
313  KDL_DIFF(2.0*t,t2);
314  t2-=t;
315  KDL_DIFF(t,t2);
316  t.ReverseSign();
317  KDL_DIFF(t,-t2);
318  KDL_DIFF(R.Inverse(R*t),t);
319  KDL_DIFF(R*t,R*R.Inverse(R*t));
320  KDL_DIFF(F.Inverse(F*t),t);
321  KDL_DIFF(F*t,F*F.Inverse(F*t));
322  KDL_DIFF(doubleAcc(3.14,2,3)*t,t*doubleAcc(3.14,2,3));
323  KDL_DIFF(t/doubleAcc(3.14,2,7),t*(1.0/doubleAcc(3.14,2,7)));
324  KDL_DIFF(t/3.14,t*(1.0/3.14));
325  KDL_DIFF(-t,-1.0*t);
326  VectorAcc p1(Vector(5,1,2),Vector(4,2,1),Vector(2,1,3));
327  VectorAcc p2(Vector(2,0,5),Vector(-2,7,-1),Vector(-3,2,-1));
328  KDL_DIFF(t.RefPoint(p1+p2),t.RefPoint(p1).RefPoint(p2));
329  KDL_DIFF(t,t.RefPoint(-p1).RefPoint(p1));
330 }
331 
332 int main() {
333  KDL_CTX;
334  TestVector();
335  TestRotation();
336  TestFrame();
337  TestVectorVel();
338  TestRotationVel();
339  TestFrameVel();
340  TestVectorAcc();
341  TestRotationAcc();
342  TestFrameAcc();
343  TestTwistVel();
344  TestTwistAcc();
345  return 0;
346 }
TestRotationAcc
void TestRotationAcc()
Definition: rframestest.cpp:134
TestVector
void TestVector()
Definition: rframestest.cpp:10
TestTwistAcc
void TestTwistAcc()
Definition: rframestest.cpp:290
KDL::VectorAcc::ReverseSign
IMETHOD void ReverseSign()
Definition: frameacc.hpp:124
KDL::Rotation
represents rotations in 3 dimensional space.
Definition: frames.hpp:301
TestFrameAcc
void TestFrameAcc()
Definition: rframestest.cpp:214
KDL::RotationAcc::Identity
static IMETHOD RotationAcc Identity()
Definition: frameacc.hpp:221
KDL::Norm
INLINE S Norm(const Rall1d< T, V, S > &value)
Definition: rall1d.h:418
KDL::Rotation::Inverse
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
Definition: frames.hpp:633
KDL::RotationVel::Identity
static IMETHOD RotationVel Identity()
Definition: framevel.hpp:132
TestRotation
void TestRotation()
Definition: rframestest.cpp:73
KDL::TwistAcc::ReverseSign
IMETHOD void ReverseSign()
Definition: frameacc.hpp:441
TestRotationVel
void TestRotationVel()
Definition: rframestest.cpp:104
KDL::TwistVel::ReverseSign
IMETHOD void ReverseSign()
Definition: framevel.hpp:385
KDL::Vector::ReverseSign
void ReverseSign()
Reverses the sign of the Vector object itself.
Definition: frames.hpp:441
KDL::dot
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
Definition: frameacc.hpp:138
TestFrameVel
void TestFrameVel()
Definition: rframestest.cpp:188
KDL::Rotation::EulerZYX
static Rotation EulerZYX(double Alfa, double Beta, double Gamma)
Definition: frames.hpp:469
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:720
KDL::RotationAcc::Inverse
IMETHOD RotationAcc Inverse() const
Definition: frameacc.hpp:225
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:160
KDL::Rotation::RPY
static Rotation RPY(double roll, double pitch, double yaw)
Definition: frames.cpp:238
TestFrame
void TestFrame()
Definition: rframestest.cpp:164
TestTwistVel
void TestTwistVel()
Definition: rframestest.cpp:241
KDL::FrameVel::Identity
static IMETHOD FrameVel Identity()
Definition: framevel.hpp:29
TestVectorVel
void TestVectorVel()
Definition: rframestest.cpp:31
KDL::RotationVel::Inverse
IMETHOD RotationVel Inverse() const
Definition: framevel.hpp:136
KDL::Rotation::Identity
static Rotation Identity()
Gives back an identity rotaton matrix.
Definition: frames.hpp:548
KDL::FrameAcc::Identity
static IMETHOD FrameAcc Identity()
Definition: frameacc.hpp:345
KDL::sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:369
KDL::Frame::Inverse
Frame Inverse() const
Gives back inverse transformation of a Frame.
Definition: frames.hpp:423
KDL::doubleAcc
Rall2d< double, double, double > doubleAcc
Definition: frameacc.hpp:40
KDL::Frame::Identity
static Frame Identity()
Definition: frames.hpp:696
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
KDL::FrameVel::Inverse
IMETHOD FrameVel Inverse() const
Definition: framevel.hpp:66
KDL::FrameAcc::Inverse
IMETHOD FrameAcc Inverse() const
Definition: frameacc.hpp:382
KDL::deg2rad
const double deg2rad
the value pi/180
KDL::VectorVel::ReverseSign
IMETHOD void ReverseSign()
Definition: framevel.hpp:315
KDL::doubleVel
Rall1d< double > doubleVel
Definition: framevel.hpp:36
TestVectorAcc
void TestVectorAcc()
Definition: rframestest.cpp:52
main
int main()
Definition: rframestest.cpp:332

orocos_kdl
Author(s):
autogenerated on Sat Jun 15 2019 19:07:36