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

orocos_kdl
Author(s):
autogenerated on Thu Apr 13 2023 02:19:14