framestest.cpp
Go to the documentation of this file.
1 #include <math.h>
2 #include "framestest.hpp"
3 #include <frames_io.hpp>
4 CPPUNIT_TEST_SUITE_REGISTRATION( FramesTest );
5 
6 using namespace KDL;
7 
8 void FramesTest::setUp()
9 {
10 }
11 
12 void FramesTest::tearDown()
13 {
14 }
15 
16 void FramesTest::TestVector2(Vector& v) {
17  Vector v2;
18  CPPUNIT_ASSERT_EQUAL(2*v-v,v);
19  CPPUNIT_ASSERT_EQUAL(v*2-v,v);
20  CPPUNIT_ASSERT_EQUAL(v+v+v-2*v,v);
21  v2=v;
22  CPPUNIT_ASSERT_EQUAL(v,v2);
23  v2+=v;
24  CPPUNIT_ASSERT_EQUAL(2*v,v2);
25  v2-=v;
26  CPPUNIT_ASSERT_EQUAL(v,v2);
27  v2.ReverseSign();
28  CPPUNIT_ASSERT_EQUAL(v,-v2);
29 }
30 
31 void FramesTest::TestVector() {
32  Vector v(3,4,5);
33  TestVector2(v);
34  Vector v2(0,0,0);
35  TestVector2(v2);
36 
37  Vector nu(0,0,0);
38  CPPUNIT_ASSERT_EQUAL(nu.Norm(),0.0);
39  Vector nu2(10,0,0);
40  CPPUNIT_ASSERT_EQUAL(nu2.Norm(),10.0);
41 }
42 
43 void FramesTest::TestVector2DNorm() {
44  Vector2 nu(0, 0);
45  CPPUNIT_ASSERT_EQUAL(nu.Norm(), 0.0);
46 
47  CPPUNIT_ASSERT_EQUAL(Vector2(1, 0).Norm(), 1.0);
48  CPPUNIT_ASSERT_EQUAL(Vector2(0, 1).Norm(), 1.0);
49  CPPUNIT_ASSERT_EQUAL(Vector2(-1, 0).Norm(), 1.0);
50  CPPUNIT_ASSERT_EQUAL(Vector2(0, -1).Norm(), 1.0);
51 }
52 
53 void FramesTest::TestTwist2(Twist& t) {
54  Twist t2(Vector(16,-3,5),Vector(-4,2,1));
55 
56  CPPUNIT_ASSERT_EQUAL(2*t-t,t);
57  CPPUNIT_ASSERT_EQUAL(t*2-t,t);
58  CPPUNIT_ASSERT_EQUAL(t+t+t-2*t,t);
59  t2=t;
60  CPPUNIT_ASSERT_EQUAL(t,t2);
61  t2+=t;
62  CPPUNIT_ASSERT_EQUAL(2*t,t2);
63  t2-=t;
64  CPPUNIT_ASSERT_EQUAL(t,t2);
65  t.ReverseSign();
66  CPPUNIT_ASSERT_EQUAL(t,-t2);
67 }
68 
69 void FramesTest::TestTwist() {
70  Twist t(Vector(6,3,5),Vector(4,-2,7));
71  TestTwist2(t);
72  Twist t2(Vector(0,0,0),Vector(0,0,0));
73  TestTwist2(t2);
74  Twist t3(Vector(0,-9,-3),Vector(1,-2,-4));
75  TestTwist2(t3);
76 }
77 
78 void FramesTest::TestWrench2(Wrench& w) {
79  // Wrench
80  Wrench w2;
81  CPPUNIT_ASSERT_EQUAL(2*w-w,w);
82  CPPUNIT_ASSERT_EQUAL(w*2-w,w);
83  CPPUNIT_ASSERT_EQUAL(w+w+w-2*w,w);
84  w2=w;
85  CPPUNIT_ASSERT_EQUAL(w,w2);
86  w2+=w;
87  CPPUNIT_ASSERT_EQUAL(2*w,w2);
88  w2-=w;
89  CPPUNIT_ASSERT_EQUAL(w,w2);
90  w.ReverseSign();
91  CPPUNIT_ASSERT_EQUAL(w,-w2);
92 }
93 
94 void FramesTest::TestWrench() {
95  Wrench w(Vector(7,-1,3),Vector(2,-3,3));
96  TestWrench2(w);
97  Wrench w2(Vector(0,0,0),Vector(0,0,0));
98  TestWrench2(w2);
99  Wrench w3(Vector(2,1,4),Vector(5,3,1));
100  TestWrench2(w3);
101 }
102 
103 void FramesTest::TestRotation2(const Vector& v,double a,double b,double c) {
104  Vector v2;
105  // Wrench
106  Wrench w(Vector(7,-1,3),Vector(2,-3,3));
107  Twist t(Vector(6,3,5),Vector(4,-2,7));
108  // Rotation
109  Rotation R;
110  Rotation R2;
111  double ra;
112  double rb;
113  double rc;
114  R = Rotation::RPY(a,b,c);
115 
116  CPPUNIT_ASSERT_DOUBLES_EQUAL(dot(R.UnitX(),R.UnitX()),1.0,epsilon);
117  CPPUNIT_ASSERT_DOUBLES_EQUAL(dot(R.UnitY(),R.UnitY()),1.0,epsilon);
118  CPPUNIT_ASSERT_DOUBLES_EQUAL(dot(R.UnitZ(),R.UnitZ()),1.0,epsilon);
119  CPPUNIT_ASSERT_DOUBLES_EQUAL(dot(R.UnitX(),R.UnitY()),0.0,epsilon);
120  CPPUNIT_ASSERT_DOUBLES_EQUAL(dot(R.UnitX(),R.UnitZ()),0.0,epsilon);
121  CPPUNIT_ASSERT_DOUBLES_EQUAL(dot(R.UnitY(),R.UnitZ()),0.0,epsilon);
122  R2=R;
123  CPPUNIT_ASSERT_EQUAL(R,R2);
124  CPPUNIT_ASSERT_DOUBLES_EQUAL((R*v).Norm(),v.Norm(),epsilon);
125  CPPUNIT_ASSERT_EQUAL(R.Inverse(R*v),v);
126  CPPUNIT_ASSERT_EQUAL(R.Inverse(R*t),t);
127  CPPUNIT_ASSERT_EQUAL(R.Inverse(R*w),w);
128  CPPUNIT_ASSERT_EQUAL(R*R.Inverse(v),v);
129  CPPUNIT_ASSERT_EQUAL(R*Rotation::Identity(),R);
130  CPPUNIT_ASSERT_EQUAL(Rotation::Identity()*R,R);
131  CPPUNIT_ASSERT_EQUAL(R*(R*(R*v)),(R*R*R)*v);
132  CPPUNIT_ASSERT_EQUAL(R*(R*(R*t)),(R*R*R)*t);
133  CPPUNIT_ASSERT_EQUAL(R*(R*(R*w)),(R*R*R)*w);
134  CPPUNIT_ASSERT_EQUAL(R*R.Inverse(),Rotation::Identity());
135  CPPUNIT_ASSERT_EQUAL(R.Inverse()*R,Rotation::Identity());
136  CPPUNIT_ASSERT_EQUAL(R.Inverse()*v,R.Inverse(v));
137  R.GetRPY(ra,rb,rc);
138  CPPUNIT_ASSERT_DOUBLES_EQUAL(ra,a,epsilon);
139  CPPUNIT_ASSERT_DOUBLES_EQUAL(rb,b,epsilon);
140  CPPUNIT_ASSERT_DOUBLES_EQUAL(rc,c,epsilon);
141  R = Rotation::EulerZYX(a,b,c);
142  R.GetEulerZYX(ra,rb,rc);
143  CPPUNIT_ASSERT_DOUBLES_EQUAL(ra,a,epsilon);
144  CPPUNIT_ASSERT_DOUBLES_EQUAL(rb,b,epsilon);
145  CPPUNIT_ASSERT_DOUBLES_EQUAL(rc,c,epsilon);
146  R = Rotation::EulerZYZ(a,b,c);
147  R.GetEulerZYZ(ra,rb,rc);
148  CPPUNIT_ASSERT_DOUBLES_EQUAL(ra,a,epsilon);
149  CPPUNIT_ASSERT_DOUBLES_EQUAL(rb,b,epsilon);
150  CPPUNIT_ASSERT_DOUBLES_EQUAL(rc,c,epsilon);
151  double angle= R.GetRotAngle(v2);
152  R2=Rotation::Rot2(v2,angle);
153  CPPUNIT_ASSERT_EQUAL(R2,R);
154  R2=Rotation::Rot(v2*1E20,angle);
155  CPPUNIT_ASSERT_EQUAL(R,R2);
156  v2=Vector(6,2,4);
157  CPPUNIT_ASSERT_DOUBLES_EQUAL((v2).Norm(),::sqrt(dot(v2,v2)),epsilon);
158 }
159 
160 
161 // compare a rotation R with the expected angle and axis
162 void FramesTest::TestOneRotation(const std::string& msg,
163  const KDL::Rotation& R,
164  const double expectedAngle,
165  const KDL::Vector& expectedAxis)
166 {
167  double angle =0;
168  Vector axis;
169 
170  angle = R.GetRotAngle(axis);
171  CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(msg, expectedAngle, angle, epsilon);
172  CPPUNIT_ASSERT_EQUAL_MESSAGE(msg, expectedAxis, axis);
173  CPPUNIT_ASSERT_EQUAL_MESSAGE(msg, expectedAngle * expectedAxis, R.GetRot());
174  CPPUNIT_ASSERT_EQUAL_MESSAGE(msg, Rotation::Rot(axis, angle), R);
175  (void)axis.Normalize();
176  CPPUNIT_ASSERT_EQUAL_MESSAGE(msg, Rotation::Rot2(axis, angle), R);
177 }
178 
179 
180 
181 void FramesTest::TestArbitraryRotation(const std::string& msg,
182  const KDL::Vector& v,
183  const double angle,
184  const double expectedAngle,
185  const KDL::Vector& expectedVector)
186 {
187  std::stringstream ss;
188  ss << "rot(" << msg << "],(" << angle << ")";
189  TestOneRotation(ss.str(), Rotation::Rot(v,angle*deg2rad), expectedAngle*deg2rad, expectedVector);
190 }
191 
192 
193 #define TESTEULERZYX(a,b,g) \
194  {\
195  double eps=1E-14;\
196  Rotation R = Rotation::EulerZYX((a),(b),(g));\
197  double alpha,beta,gamma;\
198  R.GetEulerZYX(alpha,beta,gamma);\
199  CPPUNIT_ASSERT_DOUBLES_EQUAL((a),alpha,eps);\
200  CPPUNIT_ASSERT_DOUBLES_EQUAL((b),beta,eps);\
201  CPPUNIT_ASSERT_DOUBLES_EQUAL((g),gamma,eps);\
202  }
203 
204 #define TESTEULERZYZ(a,b,g) \
205  {\
206  double eps=1E-14;\
207  Rotation R = Rotation::EulerZYZ((a),(b),(g));\
208  double alpha,beta,gamma;\
209  R.GetEulerZYZ(alpha,beta,gamma);\
210  CPPUNIT_ASSERT_DOUBLES_EQUAL((a),alpha,eps);\
211  CPPUNIT_ASSERT_DOUBLES_EQUAL((b),beta,eps);\
212  CPPUNIT_ASSERT_DOUBLES_EQUAL((g),gamma,eps);\
213  }
214 #define TESTEULERZYX_INVARIANT(a,b,g,a2,b2,g2)\
215  {\
216  double eps=1E-14;\
217  Rotation R1=Rotation::EulerZYX(a,b,g);\
218  Rotation R2=Rotation::EulerZYX(a2,b2,g2);\
219  CPPUNIT_ASSERT_DOUBLES_EQUAL(0,diff(R2,R1).Norm(),eps);\
220  }
221 #define TESTEULERZYZ_INVARIANT(a,b,g,a2,b2,g2)\
222  {\
223  double eps=1E-14;\
224  Rotation R1=Rotation::EulerZYZ(a,b,g);\
225  Rotation R2=Rotation::EulerZYZ(a2,b2,g2);\
226  CPPUNIT_ASSERT_DOUBLES_EQUAL(0,diff(R2,R1).Norm(),eps);\
227  }
228 void FramesTest::TestEuler() {
229  using namespace KDL;
230  TESTEULERZYX(0.1,0.2,0.3)
231  TESTEULERZYX(-0.1,0.2,0.3)
232  TESTEULERZYX(0.1,-0.2,0.3)
233  TESTEULERZYX(0.1,0.2,-0.3)
234  TESTEULERZYX(0,0.2,0.3)
235  TESTEULERZYX(0.1,0.2,0)
236  TESTEULERZYX(0.1,0,0.3)
237  TESTEULERZYX(0.1,0,0)
238  TESTEULERZYX(0,0,0.3)
239  TESTEULERZYX(0,0,0)
240  TESTEULERZYX(0.3,0.999*M_PI/2,0.1)
241  // if beta== +/- M_PI/2 => multiple solutions available, gamma will be chosen to be zero !
242  // so we test with gamma==0 !
243  TESTEULERZYX(0.3,0.9999999999*M_PI/2,0)
244  TESTEULERZYX(0.3,0.99999999*M_PI/2,0)
245  TESTEULERZYX(0.3,0.999999*M_PI/2,0)
246  TESTEULERZYX(0.3,0.9999*M_PI/2,0)
247  TESTEULERZYX(0.3,0.99*M_PI/2,0)
248  //TESTEULERZYX(0.1,-M_PI/2,0.3)
249  TESTEULERZYX(0,M_PI/2,0)
250 
251  TESTEULERZYX(0.3,-M_PI/2,0)
252  TESTEULERZYX(0.3,-0.9999999999*M_PI/2,0)
253  TESTEULERZYX(0.3,-0.99999999*M_PI/2,0)
254  TESTEULERZYX(0.3,-0.999999*M_PI/2,0)
255  TESTEULERZYX(0.3,-0.9999*M_PI/2,0)
256  TESTEULERZYX(0.3,-0.99*M_PI/2,0)
257  TESTEULERZYX(0,-M_PI/2,0)
258 
259  // extremes of the range:
260  TESTEULERZYX(M_PI,-M_PI/2,0)
261  TESTEULERZYX(-M_PI,-M_PI/2,0)
262  TESTEULERZYX(M_PI,1,0)
263  TESTEULERZYX(-M_PI,1,0)
264  //TESTEULERZYX(0,-M_PI/2,M_PI) gamma will be chosen zero
265  //TESTEULERZYX(0,M_PI/2,-M_PI) gamma will be chosen zero
266  TESTEULERZYX(0,1,M_PI)
267 
268  TESTEULERZYZ(0.1,0.2,0.3)
269  TESTEULERZYZ(-0.1,0.2,0.3)
270  TESTEULERZYZ(0.1,0.9*M_PI,0.3)
271  TESTEULERZYZ(0.1,0.2,-0.3)
272  TESTEULERZYZ(0,0,0)
273  TESTEULERZYZ(0,0,0)
274  TESTEULERZYZ(0,0,0)
275  TESTEULERZYZ(PI,0,0)
276  TESTEULERZYZ(0,0.2,PI)
277  TESTEULERZYZ(0.4,PI,0)
278  TESTEULERZYZ(0,PI,0)
279  TESTEULERZYZ(PI,PI,0)
280  TESTEULERZYX(0.3,M_PI/2,0)
281  TESTEULERZYZ(0.3,0.9999999999*M_PI/2,0)
282  TESTEULERZYZ(0.3,0.99999999*M_PI/2,0)
283  TESTEULERZYZ(0.3,0.999999*M_PI/2,0)
284  TESTEULERZYZ(0.3,0.9999*M_PI/2,0)
285  TESTEULERZYZ(0.3,0.99*M_PI/2,0)
286 
287  TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1+M_PI, M_PI-0.2, 0.3+M_PI);
288  TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1-M_PI, M_PI-0.2, 0.3-M_PI);
289  TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1-M_PI, M_PI-0.2, 0.3+M_PI);
290  TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1+M_PI, M_PI-0.2, 0.3-M_PI);
291 
292  TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1+M_PI, -0.2, 0.3+M_PI);
293  TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1-M_PI, -0.2, 0.3+M_PI);
294  TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1+M_PI, -0.2, 0.3-M_PI);
295  TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1-M_PI, -0.2, 0.3-M_PI);
296 }
297 
298 void FramesTest::TestRangeArbitraryRotation(const std::string& msg,
299  const KDL::Vector& v,
300  const KDL::Vector& expectedVector)
301 {
302  TestArbitraryRotation(msg, v, 0, 0, Vector(0,0,1)); // no rotation
303  TestArbitraryRotation(msg, v, 45, 45, expectedVector);
304  TestArbitraryRotation(msg, v, 90, 90, expectedVector);
305  TestArbitraryRotation(msg, v, 179, 179, expectedVector);
306 // TestArbitraryRotation(msg, v, 180, 180, expectedVector); // sign VARIES by case because 180 degrees not
307  // full determined: it can return +/- the axis
308  // depending on what the original axis was.
309  // BOTH RESULTS ARE CORRECT.
310  TestArbitraryRotation(msg, v, 181, 179, -expectedVector); // >+180 rotation => <+180 + negative axis
311  TestArbitraryRotation(msg, v, 270, 90, -expectedVector);
312  TestArbitraryRotation(msg, v, 359, 1, -expectedVector);
313  TestArbitraryRotation(msg, v, 360, 0, Vector(0,0,1)); // no rotation
314  TestArbitraryRotation(msg, v, 361, 1, expectedVector);
315  TestArbitraryRotation(msg, v, 450, 90, expectedVector);
316  TestArbitraryRotation(msg, v, 539, 179, expectedVector);
317 // TestArbitraryRotation(msg, v, 540, 180, expectedVector); // see above
318  TestArbitraryRotation(msg, v, 541, 179, -expectedVector); // like 181
319  TestArbitraryRotation(msg, v, 630, 90, -expectedVector); // like 270
320  TestArbitraryRotation(msg, v, 719, 1, -expectedVector); // like 259
321  TestArbitraryRotation(msg, v, 720, 0, Vector(0,0,1)); // no rotation
322 
323  TestArbitraryRotation(msg, v, -45, 45, -expectedVector);
324  TestArbitraryRotation(msg, v, -90, 90, -expectedVector);
325  TestArbitraryRotation(msg, v, -179, 179, -expectedVector);
326 // TestArbitraryRotation(msg, v, -180, 180, expectedVector); // see above
327  TestArbitraryRotation(msg, v, -181, 179, expectedVector);
328  TestArbitraryRotation(msg, v, -270, 90, expectedVector);
329  TestArbitraryRotation(msg, v, -359, 1, expectedVector);
330  TestArbitraryRotation(msg, v, -360, 0, Vector(0,0,1)); // no rotation
331  TestArbitraryRotation(msg, v, -361, 1, -expectedVector);
332  TestArbitraryRotation(msg, v, -450, 90, -expectedVector);
333  TestArbitraryRotation(msg, v, -539, 179, -expectedVector);
334 // TestArbitraryRotation(msg, v, -540, 180, -expectedVector); // see above
335  TestArbitraryRotation(msg, v, -541, 179, expectedVector);
336  TestArbitraryRotation(msg, v, -630, 90, expectedVector);
337  TestArbitraryRotation(msg, v, -719, 1, expectedVector);
338  TestArbitraryRotation(msg, v, -720, 0, Vector(0,0,1)); // no rotation
339 }
340 
341 void FramesTest::TestRotation() {
342  TestRotation2(Vector(3,4,5),10*deg2rad,20*deg2rad,30*deg2rad);
343 
344  // X axis
345  TestRangeArbitraryRotation("[1,0,0]", Vector(1,0,0), Vector(1,0,0));
346  TestRangeArbitraryRotation("[-1,0,0]", Vector(-1,0,0), Vector(-1,0,0));
347  // Y axis
348  TestRangeArbitraryRotation("[0,1,0]", Vector(0,1,0), Vector(0,1,0));
349  TestRangeArbitraryRotation("[0,-1,0]", Vector(0,-1,0), Vector(0,-1,0));
350  // Z axis
351  TestRangeArbitraryRotation("[0,0,1]", Vector(0,0,1), Vector(0,0,1));
352  TestRangeArbitraryRotation("[0,0,-1]", Vector(0,0,-1), Vector(0,0,-1));
353  // X,Y axes
354  TestRangeArbitraryRotation("[1,1,0]", Vector(1,1,0), Vector(1,1,0)/sqrt(2.0));
355  TestRangeArbitraryRotation("[-1,-1,0]", Vector(-1,-1,0), Vector(-1,-1,0)/sqrt(2.0));
356  // X,Z axes
357  TestRangeArbitraryRotation("[1,0,1]", Vector(1,0,1), Vector(1,0,1)/sqrt(2.0));
358  TestRangeArbitraryRotation("[-1,0,-1]", Vector(-1,0,-1), Vector(-1,0,-1)/sqrt(2.0));
359  // Y,Z axes
360  TestRangeArbitraryRotation("[0,1,1]", Vector(0,1,1), Vector(0,1,1)/sqrt(2.0));
361  TestRangeArbitraryRotation("[0,-1,-1]", Vector(0,-1,-1), Vector(0,-1,-1)/sqrt(2.0));
362  // X,Y,Z axes
363  TestRangeArbitraryRotation("[1,1,1]", Vector(1,1,1), Vector(1,1,1)/sqrt(3.0));
364  TestRangeArbitraryRotation("[-1,-1,-1]", Vector(-1,-1,-1), Vector(-1,-1,-1)/sqrt(3.0));
365 
366  // these change ... some of the -180 are the same as the +180, and some
367  // results are the opposite sign.
368  TestOneRotation("rot([1,0,0],180)", KDL::Rotation::Rot(KDL::Vector(1,0,0),180*deg2rad), 180*deg2rad, Vector(1,0,0));
369  // same as +180
370  TestOneRotation("rot([-1,0,0],180)", KDL::Rotation::Rot(KDL::Vector(-1,0,0),180*deg2rad), 180*deg2rad, Vector(1,0,0));
371  TestOneRotation("rot([0,1,0],180)", KDL::Rotation::Rot(KDL::Vector(0,1,0),180*deg2rad), 180*deg2rad, Vector(0,1,0));
372  // same as +180
373  TestOneRotation("rot([0,-1,0],180)", KDL::Rotation::Rot(KDL::Vector(0,-1,0),180*deg2rad), 180*deg2rad, Vector(0,1,0));
374 
375  TestOneRotation("rot([0,0,1],180)", KDL::Rotation::Rot(KDL::Vector(0,0,1),180*deg2rad), 180*deg2rad, Vector(0,0,1));
376  // same as +180
377  TestOneRotation("rot([0,0,-1],180)", KDL::Rotation::Rot(KDL::Vector(0,0,-1),180*deg2rad), 180*deg2rad, Vector(0,0,1));
378 
379  TestOneRotation("rot([1,0,1],180)", KDL::Rotation::Rot(KDL::Vector(1,0,1),180*deg2rad), 180*deg2rad, Vector(1,0,1)/sqrt(2.0));
380  // same as +180
381  TestOneRotation("rot([1,0,1],-180)", KDL::Rotation::Rot(KDL::Vector(1,0,1),-180*deg2rad), 180*deg2rad, Vector(1,0,1)/sqrt(2.0));
382  TestOneRotation("rot([-1,0,-1],180)", KDL::Rotation::Rot(KDL::Vector(-1,0,-1),180*deg2rad), 180*deg2rad, Vector(1,0,1)/sqrt(2.0));
383  // same as +180
384  TestOneRotation("rot([-1,0,-1],-180)", KDL::Rotation::Rot(KDL::Vector(-1,0,-1),-180*deg2rad), 180*deg2rad, Vector(1,0,1)/sqrt(2.0));
385 
386  TestOneRotation("rot([1,1,0],180)", KDL::Rotation::Rot(KDL::Vector(1,1,0),180*deg2rad), 180*deg2rad, Vector(1,1,0)/sqrt(2.0));
387  // opposite of +180
388  TestOneRotation("rot([1,1,0],-180)", KDL::Rotation::Rot(KDL::Vector(1,1,0),-180*deg2rad), 180*deg2rad, Vector(1,1,0)/sqrt(2.0));
389  TestOneRotation("rot([-1,-1,0],180)", KDL::Rotation::Rot(KDL::Vector(-1,-1,0),180*deg2rad), 180*deg2rad, Vector(1,1,0)/sqrt(2.0));
390  // opposite of +180
391  TestOneRotation("rot([-1,-1,0],-180)", KDL::Rotation::Rot(KDL::Vector(-1,-1,0),-180*deg2rad), 180*deg2rad, Vector(1,1,0)/sqrt(2.0));
392 
393  TestOneRotation("rot([0,1,1],180)", KDL::Rotation::Rot(KDL::Vector(0,1,1),180*deg2rad), 180*deg2rad, Vector(0,1,1)/sqrt(2.0));
394  // same as +180
395  TestOneRotation("rot([0,1,1],-180)", KDL::Rotation::Rot(KDL::Vector(0,1,1),-180*deg2rad), 180*deg2rad, Vector(0,1,1)/sqrt(2.0));
396  TestOneRotation("rot([0,-1,-1],180)", KDL::Rotation::Rot(KDL::Vector(0,-1,-1),180*deg2rad), 180*deg2rad, Vector(0,1,1)/sqrt(2.0));
397  // same as +180
398  TestOneRotation("rot([0,-1,-1],-180)", KDL::Rotation::Rot(KDL::Vector(0,-1,-1),-180*deg2rad), 180*deg2rad, Vector(0,1,1)/sqrt(2.0));
399 
400  TestOneRotation("rot([1,1,1],180)", KDL::Rotation::Rot(KDL::Vector(1,1,1),180*deg2rad), 180*deg2rad, Vector(1,1,1)/sqrt(3.0));
401  // same as +180
402  TestOneRotation("rot([1,1,1],-180)", KDL::Rotation::Rot(KDL::Vector(1,1,1),-180*deg2rad), 180*deg2rad, Vector(1,1,1)/sqrt(3.0));
403  TestOneRotation("rot([-1,-1,-1],180)", KDL::Rotation::Rot(KDL::Vector(-1,-1,-1),180*deg2rad), 180*deg2rad, Vector(1,1,1)/sqrt(3.0));
404  // same as +180
405  TestOneRotation("rot([-1,-1,-1],-180)", KDL::Rotation::Rot(KDL::Vector(-1,-1,-1),-180*deg2rad), 180*deg2rad, Vector(1,1,1)/sqrt(3.0));
406 
407  TestOneRotation("rot([0.707107, 0, 0.707107", KDL::Rotation::RPY(-2.9811968953315162, -atan(1)*2, -0.1603957582582825), 180*deg2rad, Vector(0.707107,0,0.707107) );
408 }
409 
410 void FramesTest::TestGetRotAngle() {
411  static const double pi = atan(1)*4;
412  double roll = -2.9811968953315162;
413  double pitch = -pi/2;
414  double yaw = -0.1603957582582825;
415 
416  // rpy -> rotation
417  KDL::Rotation kdlRotation1 = KDL::Rotation::RPY(roll, pitch, yaw);
418 
419  // rotation -> angle-axis (with KDL::GetRotAngle)
420  KDL::Vector kdlAxis;
421  double theta = kdlRotation1.GetRotAngle(kdlAxis);
422 
423 
424  CPPUNIT_ASSERT(0==isnan(theta));
425  CPPUNIT_ASSERT(0==isnan(kdlAxis[0]));
426  CPPUNIT_ASSERT(0==isnan(kdlAxis[1]));
427  CPPUNIT_ASSERT(0==isnan(kdlAxis[2]));
428 
429  // Test GetRotAngle on slightly non-orthogonal rotation matrices
430  {
431  Vector axis;
432  double angle = KDL::Rotation( 1, 0, 0 + 1e-6, 0, 1, 0, 0, 0, 1 + 1e-6).GetRotAngle(axis);
433  CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE("rot(NON-ORTHOGONAL, 0)", 0.0, angle, epsilon);
434  }
435 
436  {
437  Vector axis;
438  double angle = KDL::Rotation(-1, 0, 0 + 1e-6, 0, 1, 0, 0, 0, -1 - 1e-6).GetRotAngle(axis);
439  CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE("rot(NON-ORTHOGONAL, PI)", M_PI, angle, epsilon);
440  }
441 
442  // Tests to show that GetRotAngle does not work for an improper rotation matrix which has a determinant of -1;
443  // an improper rotation matrix corresponds to a rotation between a right-hand and left-hand coordinate system
444  {
445  Vector axis;
446  double angle;
447  Rotation R, Rout;
448  double det;
449  // Improper Rotation Matrix for 120 deg rotation
450  R = KDL::Rotation( 0, -1, 0, 0, 0, -1, -1, 0, 0);
451  det = +R(0,0)*(R(1,1)*R(2,2)-R(2,1)*R(1,2))-R(0,1)*(R(1,0)*R(2,2)-R(2,0)*R(1,2))+R(0,2)*(R(1,0)*R(2,1)-R(2,0)*R(1,1));
452  CPPUNIT_ASSERT_EQUAL(det,-1.0);
453  angle = R.GetRotAngle(axis);
454  Rout = KDL::Rotation::Rot(axis, angle);
455  CPPUNIT_ASSERT_ASSERTION_FAIL(CPPUNIT_ASSERT_EQUAL(R,Rout));
456  // Improper Rotation matrix for 180 deg rotation (singular)
457  R = KDL::Rotation( -1, 0, 0, 0, -1, 0, 0, 0, -1);
458  det = +R(0,0)*(R(1,1)*R(2,2)-R(2,1)*R(1,2))-R(0,1)*(R(1,0)*R(2,2)-R(2,0)*R(1,2))+R(0,2)*(R(1,0)*R(2,1)-R(2,0)*R(1,1));
459  CPPUNIT_ASSERT_EQUAL(det,-1.0);
460  angle = R.GetRotAngle(axis);
461  Rout = KDL::Rotation::Rot(axis, angle);
462  CPPUNIT_ASSERT_ASSERTION_FAIL(CPPUNIT_ASSERT_EQUAL(R,Rout));
463  }
464 
465 }
466 
467 void FramesTest::TestQuaternion() {
468  Rotation R;
469  Rotation R2;
470  double x,y,z,w;
471  double x2,y2,z2,w2;
472 
473  // identity R -> quat -> R2
474  R.GetQuaternion(x,y,z,w);
475  R2.Quaternion(x,y,z,w);
476  CPPUNIT_ASSERT_EQUAL(R,R2);
477 
478  // 45 deg rotation in X
479  R = Rotation::EulerZYX(0,0,45*deg2rad);
480  R.GetQuaternion(x,y,z,w);
481  CPPUNIT_ASSERT_DOUBLES_EQUAL(x, sin((45*deg2rad)/2), epsilon);
482  CPPUNIT_ASSERT_DOUBLES_EQUAL(y, 0, epsilon);
483  CPPUNIT_ASSERT_DOUBLES_EQUAL(z, 0, epsilon);
484  CPPUNIT_ASSERT_DOUBLES_EQUAL(w, cos((45*deg2rad)/2), epsilon);
485  R2 = Rotation::Quaternion(x,y,z,w);
486  CPPUNIT_ASSERT_EQUAL(R,R2);
487 
488  // direct 45 deg rotation in X
489  R2 = Rotation::Quaternion(sin((45*deg2rad)/2), 0, 0, cos((45*deg2rad)/2));
490  CPPUNIT_ASSERT_EQUAL(R,R2);
491  R2.GetQuaternion(x2,y2,z2,w2);
492  CPPUNIT_ASSERT_DOUBLES_EQUAL(x, x2, epsilon);
493  CPPUNIT_ASSERT_DOUBLES_EQUAL(y, y2, epsilon);
494  CPPUNIT_ASSERT_DOUBLES_EQUAL(z, z2, epsilon);
495  CPPUNIT_ASSERT_DOUBLES_EQUAL(w, w2, epsilon);
496 
497  // 45 deg rotation in X and in Z
498  R = Rotation::EulerZYX(45*deg2rad,0,45*deg2rad);
499  R.GetQuaternion(x,y,z,w);
500  R2 = Rotation::Quaternion(x,y,z,w);
501  CPPUNIT_ASSERT_EQUAL(R,R2);
502  R2.GetQuaternion(x2,y2,z2,w2);
503  CPPUNIT_ASSERT_DOUBLES_EQUAL(x, x2, epsilon);
504  CPPUNIT_ASSERT_DOUBLES_EQUAL(y, y2, epsilon);
505  CPPUNIT_ASSERT_DOUBLES_EQUAL(z, z2, epsilon);
506  CPPUNIT_ASSERT_DOUBLES_EQUAL(w, w2, epsilon);
507 }
508 
509 
510 void FramesTest::TestRotationDiff() {
511 
512  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(0*deg2rad)), Vector(0,0,0)); // no rotation
513  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(90*deg2rad)), Vector(M_PI/2,0,0));
514  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(180*deg2rad)), Vector(M_PI,0,0));
515  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(270*deg2rad)), Vector(-M_PI/2,0,0));
516  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(360*deg2rad)), Vector(0,0,0)); // no rotation
517  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-360*deg2rad)), Vector(0,0,0)); // no rotation
518  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-270*deg2rad)), Vector(M_PI/2,0,0));
519  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-180*deg2rad)), Vector(M_PI,0,0));
520  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-90*deg2rad)), Vector(-M_PI/2,0,0));
521  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-0*deg2rad)), Vector(0,0,0)); // no rotation
522 
523  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(0*deg2rad)), Vector(0,0,0)); // no rotation
524  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(90*deg2rad)), Vector(0,M_PI/2,0));
525  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(180*deg2rad)), Vector(0,M_PI,0));
526  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(270*deg2rad)), Vector(0,-M_PI/2,0));
527  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(360*deg2rad)), Vector(0,0,0)); // no rotation
528  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-360*deg2rad)), Vector(0,0,0)); // no rotation
529  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-270*deg2rad)), Vector(0,M_PI/2,0));
530  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-180*deg2rad)), Vector(0,M_PI,0));
531  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-90*deg2rad)), Vector(0,-M_PI/2,0));
532  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-0*deg2rad)), Vector(0,0,0)); // no rotation
533 
534  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(0*deg2rad)), Vector(0,0,0)); // no rotation
535  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(90*deg2rad)), Vector(0,0,M_PI/2));
536  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(180*deg2rad)), Vector(0,0,M_PI));
537  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(270*deg2rad)), Vector(0,0,-M_PI/2));
538  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(360*deg2rad)), Vector(0,0,0)); // no rotation
539  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-360*deg2rad)), Vector(0,0,0)); // no rotation
540  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-270*deg2rad)), Vector(0,0,M_PI/2));
541  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-180*deg2rad)), Vector(0,0,M_PI));
542  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-90*deg2rad)), Vector(0,0,-M_PI/2));
543  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-0*deg2rad)), Vector(0,0,0)); // no rotation
544 
545  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotZ(90*deg2rad)), Vector(0,0,M_PI/2));
546  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotY(90*deg2rad)), Vector(0,M_PI/2,0));
547  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotZ(90*deg2rad)), Vector(0,0,M_PI/2));
548 
549  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::Identity(), Rotation::RotX(90*deg2rad)), Vector(M_PI/2,0,0));
550  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::Identity(), Rotation::RotY(90*deg2rad)), Vector(0,M_PI/2,0));
551  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::Identity(), Rotation::RotZ(90*deg2rad)), Vector(0,0,M_PI/2));
552 
553  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RPY(+0*deg2rad,0,-90*deg2rad),
554  Rotation::RPY(-0*deg2rad,0,+90*deg2rad)),
555  Vector(0,0,M_PI));
556  CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RPY(+5*deg2rad,0,-0*deg2rad),
557  Rotation::RPY(-5*deg2rad,0,+0*deg2rad)),
558  Vector(-10*deg2rad,0,0));
559 
560  KDL::Rotation R1 = Rotation::RPY(+5*deg2rad,0,-90*deg2rad);
561  CPPUNIT_ASSERT_EQUAL(KDL::diff(R1, Rotation::RPY(-5*deg2rad,0,+90*deg2rad)),
562  R1*Vector(0, 0, 180*deg2rad));
563 }
564 
565 void FramesTest::TestFrame() {
566  Vector v(3,4,5);
567  Wrench w(Vector(7,-1,3),Vector(2,-3,3)) ;
568  Twist t(Vector(6,3,5),Vector(4,-2,7)) ;
569  Rotation R ;
570  Frame F;
571  Frame F2 ;
572  F = Frame(Rotation::EulerZYX(10*deg2rad,20*deg2rad,-10*deg2rad),Vector(4,-2,1));
573  F2=F ;
574  CPPUNIT_ASSERT_EQUAL(F,F2);
575  CPPUNIT_ASSERT_EQUAL(F.Inverse(F*v),v);
576  CPPUNIT_ASSERT_EQUAL(F.Inverse(F*t),t);
577  CPPUNIT_ASSERT_EQUAL(F.Inverse(F*w),w);
578  CPPUNIT_ASSERT_EQUAL(F*F.Inverse(v),v);
579  CPPUNIT_ASSERT_EQUAL(F*F.Inverse(t),t);
580  CPPUNIT_ASSERT_EQUAL(F*F.Inverse(w),w);
581  CPPUNIT_ASSERT_EQUAL(F*Frame::Identity(),F);
582  CPPUNIT_ASSERT_EQUAL(Frame::Identity()*F,F);
583  CPPUNIT_ASSERT_EQUAL(F*(F*(F*v)),(F*F*F)*v);
584  CPPUNIT_ASSERT_EQUAL(F*(F*(F*t)),(F*F*F)*t);
585  CPPUNIT_ASSERT_EQUAL(F*(F*(F*w)),(F*F*F)*w);
586  CPPUNIT_ASSERT_EQUAL(F*F.Inverse(),Frame::Identity());
587  CPPUNIT_ASSERT_EQUAL(F.Inverse()*F,Frame::Identity());
588  CPPUNIT_ASSERT_EQUAL(F.Inverse()*v,F.Inverse(v));
589 }
590 
591 void FramesTest::TestJntArray()
592 {
593  JntArray a1(4);
594  random(a1(0));
595  random(a1(1));
596  random(a1(2));
597  random(a1(3));
598  JntArray a2(a1);
599  CPPUNIT_ASSERT(Equal(a2,a1));
600 
601  SetToZero(a2);
602  CPPUNIT_ASSERT(!Equal(a1,a2));
603 
604  JntArray a3(4);
605  CPPUNIT_ASSERT(Equal(a2,a3));
606 
607  a1=a2;
608  CPPUNIT_ASSERT(Equal(a1,a3));
609 
610  random(a1(0));
611  random(a1(1));
612  random(a1(2));
613  random(a1(3));
614 
615  Add(a1,a2,a3);
616  CPPUNIT_ASSERT(Equal(a1,a3));
617 
618  random(a2(0));
619  random(a2(1));
620  random(a2(2));
621  random(a2(3));
622  Add(a1,a2,a3);
623  Subtract(a3,a2,a3);
624  CPPUNIT_ASSERT(Equal(a1,a3));
625 
626  Multiply(a1,2,a3);
627  Add(a1,a1,a2);
628  CPPUNIT_ASSERT(Equal(a2,a3));
629 
630  double a;
631  random(a);
632  Multiply(a1,a,a3);
633  Divide(a3,a,a2);
634  CPPUNIT_ASSERT(Equal(a2,a1));
635 }
636 
637 
638 void FramesTest::TestJntArrayWhenEmpty()
639 {
640  JntArray a1;
641  JntArray a2;
642  JntArray a3(a2);
643 
644  // won't assert()
645  CPPUNIT_ASSERT_EQUAL((unsigned int)0,a1.rows());
646  CPPUNIT_ASSERT(Equal(a2,a1));
647 
648  a2 = a1;
649  CPPUNIT_ASSERT(Equal(a2,a1));
650 
651  SetToZero(a2);
652  CPPUNIT_ASSERT(Equal(a2,a1));
653 
654  Add(a1,a2,a3);
655  CPPUNIT_ASSERT(Equal(a1,a3));
656 
657  Subtract(a1,a2,a3);
658  CPPUNIT_ASSERT(Equal(a1,a3));
659 
660  Multiply(a1,3.1,a3);
661  CPPUNIT_ASSERT(Equal(a1,a3));
662 
663  Divide(a1,3.1,a3);
664  CPPUNIT_ASSERT(Equal(a1,a3));
665 
666  // MultiplyJacobian() - not tested here
667 
668 
669  /* will assert() - not tested here
670  double j1 = a1(0);
671  */
672 
673  // and now resize, and do just a few tests
674  a1.resize(3);
675  a2.resize(3);
676  CPPUNIT_ASSERT_EQUAL((unsigned int)3,a1.rows());
677  CPPUNIT_ASSERT(Equal(a2,a1));
678 
679  random(a1(0));
680  random(a1(1));
681  random(a1(2));
682  a1 = a2;
683  CPPUNIT_ASSERT(Equal(a1,a2));
684  CPPUNIT_ASSERT_EQUAL(a1(1),a2(1));
685 
686  a3.resize(3);
687  Subtract(a1,a2,a3); // a3 = a2 - a1 = {0}
688  SetToZero(a1);
689  CPPUNIT_ASSERT(Equal(a1,a3));
690 }
691 
692 
KDL::Subtract
void Subtract(const JntArray &src1, const JntArray &src2, JntArray &dest)
Definition: jntarray.cpp:87
FramesTest::TestWrench2
void TestWrench2(Wrench &w)
Definition: framestest.cpp:78
FramesTest::TestGetRotAngle
void TestGetRotAngle()
Definition: framestest.cpp:410
TESTEULERZYZ
#define TESTEULERZYZ(a, b, g)
Definition: framestest.cpp:204
KDL::Rotation
represents rotations in 3 dimensional space.
Definition: frames.hpp:301
KDL::Divide
void Divide(const JntArray &src, const double &factor, JntArray &dest)
Definition: jntarray.cpp:97
KDL::Rotation::UnitX
Vector UnitX() const
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:510
KDL::Rotation::UnitZ
Vector UnitZ() const
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:534
KDL::Rotation::GetRotAngle
double GetRotAngle(Vector &axis, double eps=epsilon) const
Definition: frames.cpp:359
KDL::Norm
INLINE S Norm(const Rall1d< T, V, S > &value)
Definition: rall1d.h:418
KDL::Twist::ReverseSign
void ReverseSign()
Reverses the sign of the twist.
Definition: frames.hpp:297
FramesTest::TestFrame
void TestFrame()
Definition: framestest.cpp:565
KDL::JntArray::rows
unsigned int rows() const
Definition: jntarray.cpp:72
KDL::Rotation::UnitY
Vector UnitY() const
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:522
KDL::Rotation::RotX
static Rotation RotX(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.hpp:601
KDL::Rotation::Inverse
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
Definition: frames.hpp:633
TESTEULERZYZ_INVARIANT
#define TESTEULERZYZ_INVARIANT(a, b, g, a2, b2, g2)
Definition: framestest.cpp:221
TESTEULERZYX
#define TESTEULERZYX(a, b, g)
Definition: framestest.cpp:193
KDL::Vector::Normalize
double Normalize(double eps=epsilon)
Definition: frames.cpp:148
KDL::PI
const double PI
the value of pi
FramesTest::TestQuaternion
void TestQuaternion()
Definition: framestest.cpp:467
KDL::Rotation::RotZ
static Rotation RotZ(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.hpp:611
KDL::diff
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
Definition: frames.hpp:1123
KDL::Rotation::Quaternion
static Rotation Quaternion(double x, double y, double z, double w)
Definition: frames.cpp:191
KDL::Vector2
2D version of Vector
Definition: frames.hpp:959
KDL::JntArray
This class represents an fixed size array containing joint values of a KDL::Chain.
Definition: jntarray.hpp:69
CPPUNIT_TEST_SUITE_REGISTRATION
CPPUNIT_TEST_SUITE_REGISTRATION(FramesTest)
frames_io.hpp
KDL::Rotation::RotY
static Rotation RotY(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.hpp:606
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
KDL::Wrench::ReverseSign
void ReverseSign()
Reverses the sign of the current Wrench.
Definition: frames.hpp:192
KDL::Rotation::GetRot
Vector GetRot() const
Definition: frames.cpp:337
KDL::Add
void Add(const JntArray &src1, const JntArray &src2, JntArray &dest)
Definition: jntarray.cpp:82
TESTEULERZYX_INVARIANT
#define TESTEULERZYX_INVARIANT(a, b, g, a2, b2, g2)
Definition: framestest.cpp:214
KDL::Rotation::GetQuaternion
void GetQuaternion(double &x, double &y, double &z, double &w) const
Definition: frames.cpp:205
FramesTest::TestArbitraryRotation
void TestArbitraryRotation(const std::string &msg, const KDL::Vector &v, const double angle, const double expectedAngle, const KDL::Vector &expectedVector)
Definition: framestest.cpp:181
KDL::Rotation::EulerZYX
static Rotation EulerZYX(double Alfa, double Beta, double Gamma)
Definition: frames.hpp:469
FramesTest::TestRotation
void TestRotation()
Definition: framestest.cpp:341
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:720
KDL::SetToZero
IMETHOD void SetToZero(Vector &v)
Definition: frames.hpp:1062
KDL::Equal
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
Definition: frameacc.hpp:394
KDL::atan
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
Definition: rall1d.h:377
FramesTest::TestRotationDiff
void TestRotationDiff()
Definition: framestest.cpp:510
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
KDL::Vector::Norm
double Norm() const
Definition: frames.cpp:118
FramesTest::TestRangeArbitraryRotation
void TestRangeArbitraryRotation(const std::string &msg, const KDL::Vector &v, const KDL::Vector &expectedVector)
Definition: framestest.cpp:298
KDL::epsilon
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
FramesTest::TestJntArray
void TestJntArray()
Definition: framestest.cpp:591
FramesTest::setUp
void setUp()
Definition: framestest.cpp:8
KDL::Rotation::Identity
static Rotation Identity()
Gives back an identity rotaton matrix.
Definition: frames.hpp:548
FramesTest::TestEuler
void TestEuler()
Definition: framestest.cpp:228
KDL::sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:369
FramesTest::TestVector2DNorm
void TestVector2DNorm()
Definition: framestest.cpp:43
KDL::Frame::Inverse
Frame Inverse() const
Gives back inverse transformation of a Frame.
Definition: frames.hpp:423
FramesTest::TestOneRotation
void TestOneRotation(const std::string &msg, const KDL::Rotation &R, const double expectedAngle, const KDL::Vector &expectedAxis)
Definition: framestest.cpp:162
FramesTest::TestVector
void TestVector()
Definition: framestest.cpp:31
KDL::JntArray::resize
void resize(unsigned int newSize)
Definition: jntarray.cpp:55
FramesTest::TestJntArrayWhenEmpty
void TestJntArrayWhenEmpty()
Definition: framestest.cpp:638
FramesTest::TestTwist
void TestTwist()
Definition: framestest.cpp:69
KDL::Rotation::EulerZYZ
static Rotation EulerZYZ(double Alfa, double Beta, double Gamma)
Definition: frames.cpp:263
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::Wrench
represents both translational and rotational acceleration.
Definition: frames.hpp:878
FramesTest::TestWrench
void TestWrench()
Definition: framestest.cpp:94
FramesTest::TestVector2
void TestVector2(Vector &v)
Definition: framestest.cpp:16
KDL::Rotation::Rot
static Rotation Rot(const Vector &rotvec, double angle)
Definition: frames.cpp:294
KDL::deg2rad
const double deg2rad
the value pi/180
KDL::cos
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:321
KDL::Vector2::Norm
double Norm() const
Definition: frames.cpp:88
KDL::Rotation::Rot2
static Rotation Rot2(const Vector &rotvec, double angle)
Along an arbitrary axes. rotvec should be normalized.
Definition: frames.cpp:304
FramesTest::TestRotation2
void TestRotation2(const Vector &v, double a, double b, double c)
Definition: framestest.cpp:103
KDL::random
IMETHOD void random(Vector &a)
addDelta operator for displacement rotational velocity.
Definition: frames.hpp:1208
FramesTest::TestTwist2
void TestTwist2(Twist &t)
Definition: framestest.cpp:53
KDL::sin
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:313
KDL::Multiply
void Multiply(const JntArray &src, const double &factor, JntArray &dest)
Definition: jntarray.cpp:92
FramesTest::tearDown
void tearDown()
Definition: framestest.cpp:12

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