00001 #include <math.h>
00002 #include "framestest.hpp"
00003 #include <frames_io.hpp>
00004 CPPUNIT_TEST_SUITE_REGISTRATION( FramesTest );
00005
00006 using namespace KDL;
00007
00008 void FramesTest::setUp()
00009 {
00010 }
00011
00012 void FramesTest::tearDown()
00013 {
00014 }
00015
00016 void FramesTest::TestVector2(Vector& v) {
00017 Vector v2;
00018 CPPUNIT_ASSERT_EQUAL(2*v-v,v);
00019 CPPUNIT_ASSERT_EQUAL(v*2-v,v);
00020 CPPUNIT_ASSERT_EQUAL(v+v+v-2*v,v);
00021 v2=v;
00022 CPPUNIT_ASSERT_EQUAL(v,v2);
00023 v2+=v;
00024 CPPUNIT_ASSERT_EQUAL(2*v,v2);
00025 v2-=v;
00026 CPPUNIT_ASSERT_EQUAL(v,v2);
00027 v2.ReverseSign();
00028 CPPUNIT_ASSERT_EQUAL(v,-v2);
00029 }
00030
00031 void FramesTest::TestVector() {
00032 Vector v(3,4,5);
00033 TestVector2(v);
00034 Vector v2(0,0,0);
00035 TestVector2(v2);
00036
00037 Vector nu(0,0,0);
00038 CPPUNIT_ASSERT_EQUAL(nu.Norm(),0.0);
00039 Vector nu2(10,0,0);
00040 CPPUNIT_ASSERT_EQUAL(nu2.Norm(),10.0);
00041 }
00042
00043 void FramesTest::TestVector2DNorm() {
00044 Vector2 nu(0, 0);
00045 CPPUNIT_ASSERT_EQUAL(nu.Norm(), 0.0);
00046
00047 CPPUNIT_ASSERT_EQUAL(Vector2(1, 0).Norm(), 1.0);
00048 CPPUNIT_ASSERT_EQUAL(Vector2(0, 1).Norm(), 1.0);
00049 CPPUNIT_ASSERT_EQUAL(Vector2(-1, 0).Norm(), 1.0);
00050 CPPUNIT_ASSERT_EQUAL(Vector2(0, -1).Norm(), 1.0);
00051 }
00052
00053 void FramesTest::TestTwist2(Twist& t) {
00054 Twist t2(Vector(16,-3,5),Vector(-4,2,1));
00055
00056 CPPUNIT_ASSERT_EQUAL(2*t-t,t);
00057 CPPUNIT_ASSERT_EQUAL(t*2-t,t);
00058 CPPUNIT_ASSERT_EQUAL(t+t+t-2*t,t);
00059 t2=t;
00060 CPPUNIT_ASSERT_EQUAL(t,t2);
00061 t2+=t;
00062 CPPUNIT_ASSERT_EQUAL(2*t,t2);
00063 t2-=t;
00064 CPPUNIT_ASSERT_EQUAL(t,t2);
00065 t.ReverseSign();
00066 CPPUNIT_ASSERT_EQUAL(t,-t2);
00067 }
00068
00069 void FramesTest::TestTwist() {
00070 Twist t(Vector(6,3,5),Vector(4,-2,7));
00071 TestTwist2(t);
00072 Twist t2(Vector(0,0,0),Vector(0,0,0));
00073 TestTwist2(t2);
00074 Twist t3(Vector(0,-9,-3),Vector(1,-2,-4));
00075 TestTwist2(t3);
00076 }
00077
00078 void FramesTest::TestWrench2(Wrench& w) {
00079
00080 Wrench w2;
00081 CPPUNIT_ASSERT_EQUAL(2*w-w,w);
00082 CPPUNIT_ASSERT_EQUAL(w*2-w,w);
00083 CPPUNIT_ASSERT_EQUAL(w+w+w-2*w,w);
00084 w2=w;
00085 CPPUNIT_ASSERT_EQUAL(w,w2);
00086 w2+=w;
00087 CPPUNIT_ASSERT_EQUAL(2*w,w2);
00088 w2-=w;
00089 CPPUNIT_ASSERT_EQUAL(w,w2);
00090 w.ReverseSign();
00091 CPPUNIT_ASSERT_EQUAL(w,-w2);
00092 }
00093
00094 void FramesTest::TestWrench() {
00095 Wrench w(Vector(7,-1,3),Vector(2,-3,3));
00096 TestWrench2(w);
00097 Wrench w2(Vector(0,0,0),Vector(0,0,0));
00098 TestWrench2(w2);
00099 Wrench w3(Vector(2,1,4),Vector(5,3,1));
00100 TestWrench2(w3);
00101 }
00102
00103 void FramesTest::TestRotation2(const Vector& v,double a,double b,double c) {
00104 Vector v2;
00105
00106 Wrench w(Vector(7,-1,3),Vector(2,-3,3));
00107 Twist t(Vector(6,3,5),Vector(4,-2,7));
00108
00109 Rotation R;
00110 Rotation R2;
00111 double ra;
00112 double rb;
00113 double rc;
00114 R = Rotation::RPY(a,b,c);
00115
00116 CPPUNIT_ASSERT_DOUBLES_EQUAL(dot(R.UnitX(),R.UnitX()),1.0,epsilon);
00117 CPPUNIT_ASSERT_DOUBLES_EQUAL(dot(R.UnitY(),R.UnitY()),1.0,epsilon);
00118 CPPUNIT_ASSERT_DOUBLES_EQUAL(dot(R.UnitZ(),R.UnitZ()),1.0,epsilon);
00119 CPPUNIT_ASSERT_DOUBLES_EQUAL(dot(R.UnitX(),R.UnitY()),0.0,epsilon);
00120 CPPUNIT_ASSERT_DOUBLES_EQUAL(dot(R.UnitX(),R.UnitZ()),0.0,epsilon);
00121 CPPUNIT_ASSERT_DOUBLES_EQUAL(dot(R.UnitY(),R.UnitZ()),0.0,epsilon);
00122 R2=R;
00123 CPPUNIT_ASSERT_EQUAL(R,R2);
00124 CPPUNIT_ASSERT_DOUBLES_EQUAL((R*v).Norm(),v.Norm(),epsilon);
00125 CPPUNIT_ASSERT_EQUAL(R.Inverse(R*v),v);
00126 CPPUNIT_ASSERT_EQUAL(R.Inverse(R*t),t);
00127 CPPUNIT_ASSERT_EQUAL(R.Inverse(R*w),w);
00128 CPPUNIT_ASSERT_EQUAL(R*R.Inverse(v),v);
00129 CPPUNIT_ASSERT_EQUAL(R*Rotation::Identity(),R);
00130 CPPUNIT_ASSERT_EQUAL(Rotation::Identity()*R,R);
00131 CPPUNIT_ASSERT_EQUAL(R*(R*(R*v)),(R*R*R)*v);
00132 CPPUNIT_ASSERT_EQUAL(R*(R*(R*t)),(R*R*R)*t);
00133 CPPUNIT_ASSERT_EQUAL(R*(R*(R*w)),(R*R*R)*w);
00134 CPPUNIT_ASSERT_EQUAL(R*R.Inverse(),Rotation::Identity());
00135 CPPUNIT_ASSERT_EQUAL(R.Inverse()*R,Rotation::Identity());
00136 CPPUNIT_ASSERT_EQUAL(R.Inverse()*v,R.Inverse(v));
00137 R.GetRPY(ra,rb,rc);
00138 CPPUNIT_ASSERT_DOUBLES_EQUAL(ra,a,epsilon);
00139 CPPUNIT_ASSERT_DOUBLES_EQUAL(rb,b,epsilon);
00140 CPPUNIT_ASSERT_DOUBLES_EQUAL(rc,c,epsilon);
00141 R = Rotation::EulerZYX(a,b,c);
00142 R.GetEulerZYX(ra,rb,rc);
00143 CPPUNIT_ASSERT_DOUBLES_EQUAL(ra,a,epsilon);
00144 CPPUNIT_ASSERT_DOUBLES_EQUAL(rb,b,epsilon);
00145 CPPUNIT_ASSERT_DOUBLES_EQUAL(rc,c,epsilon);
00146 R = Rotation::EulerZYZ(a,b,c);
00147 R.GetEulerZYZ(ra,rb,rc);
00148 CPPUNIT_ASSERT_DOUBLES_EQUAL(ra,a,epsilon);
00149 CPPUNIT_ASSERT_DOUBLES_EQUAL(rb,b,epsilon);
00150 CPPUNIT_ASSERT_DOUBLES_EQUAL(rc,c,epsilon);
00151 double angle= R.GetRotAngle(v2);
00152 R2=Rotation::Rot2(v2,angle);
00153 CPPUNIT_ASSERT_EQUAL(R2,R);
00154 R2=Rotation::Rot(v2*1E20,angle);
00155 CPPUNIT_ASSERT_EQUAL(R,R2);
00156 v2=Vector(6,2,4);
00157 CPPUNIT_ASSERT_DOUBLES_EQUAL((v2).Norm(),::sqrt(dot(v2,v2)),epsilon);
00158 }
00159
00160
00161
00162 void FramesTest::TestOneRotation(const std::string& msg,
00163 const KDL::Rotation& R,
00164 const double expectedAngle,
00165 const KDL::Vector& expectedAxis)
00166 {
00167 double angle =0;
00168 Vector axis;
00169
00170 angle = R.GetRotAngle(axis);
00171 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(msg, expectedAngle, angle, epsilon);
00172 CPPUNIT_ASSERT_EQUAL_MESSAGE(msg, expectedAxis, axis);
00173 CPPUNIT_ASSERT_EQUAL_MESSAGE(msg, expectedAngle * expectedAxis, R.GetRot());
00174 CPPUNIT_ASSERT_EQUAL_MESSAGE(msg, Rotation::Rot(axis, angle), R);
00175 (void)axis.Normalize();
00176 CPPUNIT_ASSERT_EQUAL_MESSAGE(msg, Rotation::Rot2(axis, angle), R);
00177 }
00178
00179
00180
00181 void FramesTest::TestArbitraryRotation(const std::string& msg,
00182 const KDL::Vector& v,
00183 const double angle,
00184 const double expectedAngle,
00185 const KDL::Vector& expectedVector)
00186 {
00187 std::stringstream ss;
00188 ss << "rot(" << msg << "],(" << angle << ")";
00189 TestOneRotation(ss.str(), Rotation::Rot(v,angle*deg2rad), expectedAngle*deg2rad, expectedVector);
00190 }
00191
00192
00193 #define TESTEULERZYX(a,b,g) \
00194 {\
00195 double eps=1E-14;\
00196 Rotation R = Rotation::EulerZYX((a),(b),(g));\
00197 double alpha,beta,gamma;\
00198 R.GetEulerZYX(alpha,beta,gamma);\
00199 CPPUNIT_ASSERT_DOUBLES_EQUAL((a),alpha,eps);\
00200 CPPUNIT_ASSERT_DOUBLES_EQUAL((b),beta,eps);\
00201 CPPUNIT_ASSERT_DOUBLES_EQUAL((g),gamma,eps);\
00202 }
00203
00204 #define TESTEULERZYZ(a,b,g) \
00205 {\
00206 double eps=1E-14;\
00207 Rotation R = Rotation::EulerZYZ((a),(b),(g));\
00208 double alpha,beta,gamma;\
00209 R.GetEulerZYZ(alpha,beta,gamma);\
00210 CPPUNIT_ASSERT_DOUBLES_EQUAL((a),alpha,eps);\
00211 CPPUNIT_ASSERT_DOUBLES_EQUAL((b),beta,eps);\
00212 CPPUNIT_ASSERT_DOUBLES_EQUAL((g),gamma,eps);\
00213 }
00214 #define TESTEULERZYX_INVARIANT(a,b,g,a2,b2,g2)\
00215 {\
00216 double eps=1E-14;\
00217 Rotation R1=Rotation::EulerZYX(a,b,g);\
00218 Rotation R2=Rotation::EulerZYX(a2,b2,g2);\
00219 CPPUNIT_ASSERT_DOUBLES_EQUAL(0,diff(R2,R1).Norm(),eps);\
00220 }
00221 #define TESTEULERZYZ_INVARIANT(a,b,g,a2,b2,g2)\
00222 {\
00223 double eps=1E-14;\
00224 Rotation R1=Rotation::EulerZYZ(a,b,g);\
00225 Rotation R2=Rotation::EulerZYZ(a2,b2,g2);\
00226 CPPUNIT_ASSERT_DOUBLES_EQUAL(0,diff(R2,R1).Norm(),eps);\
00227 }
00228 void FramesTest::TestEuler() {
00229 using namespace KDL;
00230 TESTEULERZYX(0.1,0.2,0.3)
00231 TESTEULERZYX(-0.1,0.2,0.3)
00232 TESTEULERZYX(0.1,-0.2,0.3)
00233 TESTEULERZYX(0.1,0.2,-0.3)
00234 TESTEULERZYX(0,0.2,0.3)
00235 TESTEULERZYX(0.1,0.2,0)
00236 TESTEULERZYX(0.1,0,0.3)
00237 TESTEULERZYX(0.1,0,0)
00238 TESTEULERZYX(0,0,0.3)
00239 TESTEULERZYX(0,0,0)
00240 TESTEULERZYX(0.3,0.999*M_PI/2,0.1)
00241
00242
00243 TESTEULERZYX(0.3,0.9999999999*M_PI/2,0)
00244 TESTEULERZYX(0.3,0.99999999*M_PI/2,0)
00245 TESTEULERZYX(0.3,0.999999*M_PI/2,0)
00246 TESTEULERZYX(0.3,0.9999*M_PI/2,0)
00247 TESTEULERZYX(0.3,0.99*M_PI/2,0)
00248
00249 TESTEULERZYX(0,M_PI/2,0)
00250
00251 TESTEULERZYX(0.3,-M_PI/2,0)
00252 TESTEULERZYX(0.3,-0.9999999999*M_PI/2,0)
00253 TESTEULERZYX(0.3,-0.99999999*M_PI/2,0)
00254 TESTEULERZYX(0.3,-0.999999*M_PI/2,0)
00255 TESTEULERZYX(0.3,-0.9999*M_PI/2,0)
00256 TESTEULERZYX(0.3,-0.99*M_PI/2,0)
00257 TESTEULERZYX(0,-M_PI/2,0)
00258
00259
00260 TESTEULERZYX(M_PI,-M_PI/2,0)
00261 TESTEULERZYX(-M_PI,-M_PI/2,0)
00262 TESTEULERZYX(M_PI,1,0)
00263 TESTEULERZYX(-M_PI,1,0)
00264
00265
00266 TESTEULERZYX(0,1,M_PI)
00267
00268 TESTEULERZYZ(0.1,0.2,0.3)
00269 TESTEULERZYZ(-0.1,0.2,0.3)
00270 TESTEULERZYZ(0.1,0.9*M_PI,0.3)
00271 TESTEULERZYZ(0.1,0.2,-0.3)
00272 TESTEULERZYZ(0,0,0)
00273 TESTEULERZYZ(0,0,0)
00274 TESTEULERZYZ(0,0,0)
00275 TESTEULERZYZ(PI,0,0)
00276 TESTEULERZYZ(0,0.2,PI)
00277 TESTEULERZYZ(0.4,PI,0)
00278 TESTEULERZYZ(0,PI,0)
00279 TESTEULERZYZ(PI,PI,0)
00280 TESTEULERZYX(0.3,M_PI/2,0)
00281 TESTEULERZYZ(0.3,0.9999999999*M_PI/2,0)
00282 TESTEULERZYZ(0.3,0.99999999*M_PI/2,0)
00283 TESTEULERZYZ(0.3,0.999999*M_PI/2,0)
00284 TESTEULERZYZ(0.3,0.9999*M_PI/2,0)
00285 TESTEULERZYZ(0.3,0.99*M_PI/2,0)
00286
00287 TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1+M_PI, M_PI-0.2, 0.3+M_PI);
00288 TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1-M_PI, M_PI-0.2, 0.3-M_PI);
00289 TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1-M_PI, M_PI-0.2, 0.3+M_PI);
00290 TESTEULERZYX_INVARIANT(0.1,0.2,0.3, 0.1+M_PI, M_PI-0.2, 0.3-M_PI);
00291
00292 TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1+M_PI, -0.2, 0.3+M_PI);
00293 TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1-M_PI, -0.2, 0.3+M_PI);
00294 TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1+M_PI, -0.2, 0.3-M_PI);
00295 TESTEULERZYZ_INVARIANT(0.1,0.2,0.3, 0.1-M_PI, -0.2, 0.3-M_PI);
00296 }
00297
00298 void FramesTest::TestRangeArbitraryRotation(const std::string& msg,
00299 const KDL::Vector& v,
00300 const KDL::Vector& expectedVector)
00301 {
00302 TestArbitraryRotation(msg, v, 0, 0, Vector(0,0,1));
00303 TestArbitraryRotation(msg, v, 45, 45, expectedVector);
00304 TestArbitraryRotation(msg, v, 90, 90, expectedVector);
00305 TestArbitraryRotation(msg, v, 179, 179, expectedVector);
00306
00307
00308
00309
00310 TestArbitraryRotation(msg, v, 181, 179, -expectedVector);
00311 TestArbitraryRotation(msg, v, 270, 90, -expectedVector);
00312 TestArbitraryRotation(msg, v, 359, 1, -expectedVector);
00313 TestArbitraryRotation(msg, v, 360, 0, Vector(0,0,1));
00314 TestArbitraryRotation(msg, v, 361, 1, expectedVector);
00315 TestArbitraryRotation(msg, v, 450, 90, expectedVector);
00316 TestArbitraryRotation(msg, v, 539, 179, expectedVector);
00317
00318 TestArbitraryRotation(msg, v, 541, 179, -expectedVector);
00319 TestArbitraryRotation(msg, v, 630, 90, -expectedVector);
00320 TestArbitraryRotation(msg, v, 719, 1, -expectedVector);
00321 TestArbitraryRotation(msg, v, 720, 0, Vector(0,0,1));
00322
00323 TestArbitraryRotation(msg, v, -45, 45, -expectedVector);
00324 TestArbitraryRotation(msg, v, -90, 90, -expectedVector);
00325 TestArbitraryRotation(msg, v, -179, 179, -expectedVector);
00326
00327 TestArbitraryRotation(msg, v, -181, 179, expectedVector);
00328 TestArbitraryRotation(msg, v, -270, 90, expectedVector);
00329 TestArbitraryRotation(msg, v, -359, 1, expectedVector);
00330 TestArbitraryRotation(msg, v, -360, 0, Vector(0,0,1));
00331 TestArbitraryRotation(msg, v, -361, 1, -expectedVector);
00332 TestArbitraryRotation(msg, v, -450, 90, -expectedVector);
00333 TestArbitraryRotation(msg, v, -539, 179, -expectedVector);
00334
00335 TestArbitraryRotation(msg, v, -541, 179, expectedVector);
00336 TestArbitraryRotation(msg, v, -630, 90, expectedVector);
00337 TestArbitraryRotation(msg, v, -719, 1, expectedVector);
00338 TestArbitraryRotation(msg, v, -720, 0, Vector(0,0,1));
00339 }
00340
00341 void FramesTest::TestRotation() {
00342 TestRotation2(Vector(3,4,5),10*deg2rad,20*deg2rad,30*deg2rad);
00343
00344
00345 TestRangeArbitraryRotation("[1,0,0]", Vector(1,0,0), Vector(1,0,0));
00346 TestRangeArbitraryRotation("[-1,0,0]", Vector(-1,0,0), Vector(-1,0,0));
00347
00348 TestRangeArbitraryRotation("[0,1,0]", Vector(0,1,0), Vector(0,1,0));
00349 TestRangeArbitraryRotation("[0,-1,0]", Vector(0,-1,0), Vector(0,-1,0));
00350
00351 TestRangeArbitraryRotation("[0,0,1]", Vector(0,0,1), Vector(0,0,1));
00352 TestRangeArbitraryRotation("[0,0,-1]", Vector(0,0,-1), Vector(0,0,-1));
00353
00354 TestRangeArbitraryRotation("[1,1,0]", Vector(1,1,0), Vector(1,1,0)/sqrt(2.0));
00355 TestRangeArbitraryRotation("[-1,-1,0]", Vector(-1,-1,0), Vector(-1,-1,0)/sqrt(2.0));
00356
00357 TestRangeArbitraryRotation("[1,0,1]", Vector(1,0,1), Vector(1,0,1)/sqrt(2.0));
00358 TestRangeArbitraryRotation("[-1,0,-1]", Vector(-1,0,-1), Vector(-1,0,-1)/sqrt(2.0));
00359
00360 TestRangeArbitraryRotation("[0,1,1]", Vector(0,1,1), Vector(0,1,1)/sqrt(2.0));
00361 TestRangeArbitraryRotation("[0,-1,-1]", Vector(0,-1,-1), Vector(0,-1,-1)/sqrt(2.0));
00362
00363 TestRangeArbitraryRotation("[1,1,1]", Vector(1,1,1), Vector(1,1,1)/sqrt(3.0));
00364 TestRangeArbitraryRotation("[-1,-1,-1]", Vector(-1,-1,-1), Vector(-1,-1,-1)/sqrt(3.0));
00365
00366
00367
00368 TestOneRotation("rot([1,0,0],180)", KDL::Rotation::Rot(KDL::Vector(1,0,0),180*deg2rad), 180*deg2rad, Vector(1,0,0));
00369
00370 TestOneRotation("rot([-1,0,0],180)", KDL::Rotation::Rot(KDL::Vector(-1,0,0),180*deg2rad), 180*deg2rad, Vector(1,0,0));
00371 TestOneRotation("rot([0,1,0],180)", KDL::Rotation::Rot(KDL::Vector(0,1,0),180*deg2rad), 180*deg2rad, Vector(0,1,0));
00372
00373 TestOneRotation("rot([0,-1,0],180)", KDL::Rotation::Rot(KDL::Vector(0,-1,0),180*deg2rad), 180*deg2rad, Vector(0,1,0));
00374
00375 TestOneRotation("rot([0,0,1],180)", KDL::Rotation::Rot(KDL::Vector(0,0,1),180*deg2rad), 180*deg2rad, Vector(0,0,1));
00376
00377 TestOneRotation("rot([0,0,-1],180)", KDL::Rotation::Rot(KDL::Vector(0,0,-1),180*deg2rad), 180*deg2rad, Vector(0,0,1));
00378
00379 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));
00380
00381 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));
00382 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));
00383
00384 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));
00385
00386 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));
00387
00388 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));
00389 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));
00390
00391 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));
00392
00393 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));
00394
00395 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));
00396 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));
00397
00398 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));
00399
00400 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));
00401
00402 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));
00403 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));
00404
00405 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));
00406
00407 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) );
00408 }
00409
00410 void FramesTest::TestGetRotAngle() {
00411 static const double pi = atan(1)*4;
00412 double roll = -2.9811968953315162;
00413 double pitch = -pi/2;
00414 double yaw = -0.1603957582582825;
00415
00416
00417 KDL::Rotation kdlRotation1 = KDL::Rotation::RPY(roll, pitch, yaw);
00418
00419
00420 KDL::Vector kdlAxis;
00421 double theta = kdlRotation1.GetRotAngle(kdlAxis);
00422
00423
00424 CPPUNIT_ASSERT(0==isnan(theta));
00425 CPPUNIT_ASSERT(0==isnan(kdlAxis[0]));
00426 CPPUNIT_ASSERT(0==isnan(kdlAxis[1]));
00427 CPPUNIT_ASSERT(0==isnan(kdlAxis[2]));
00428
00429
00430 {
00431 Vector axis;
00432 double angle = KDL::Rotation( 1, 0, 0 + 1e-6, 0, 1, 0, 0, 0, 1 + 1e-6).GetRotAngle(axis);
00433 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE("rot(NON-ORTHOGONAL, 0)", 0.0, angle, epsilon);
00434 }
00435
00436 {
00437 Vector axis;
00438 double angle = KDL::Rotation(-1, 0, 0 + 1e-6, 0, 1, 0, 0, 0, -1 - 1e-6).GetRotAngle(axis);
00439 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE("rot(NON-ORTHOGONAL, PI)", M_PI, angle, epsilon);
00440 }
00441
00442
00443
00444 {
00445 Vector axis;
00446 double angle;
00447 Rotation R, Rout;
00448 double det;
00449
00450 R = KDL::Rotation( 0, -1, 0, 0, 0, -1, -1, 0, 0);
00451 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));
00452 CPPUNIT_ASSERT_EQUAL(det,-1.0);
00453 angle = R.GetRotAngle(axis);
00454 Rout = KDL::Rotation::Rot(axis, angle);
00455 CPPUNIT_ASSERT_ASSERTION_FAIL(CPPUNIT_ASSERT_EQUAL(R,Rout));
00456
00457 R = KDL::Rotation( -1, 0, 0, 0, -1, 0, 0, 0, -1);
00458 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));
00459 CPPUNIT_ASSERT_EQUAL(det,-1.0);
00460 angle = R.GetRotAngle(axis);
00461 Rout = KDL::Rotation::Rot(axis, angle);
00462 CPPUNIT_ASSERT_ASSERTION_FAIL(CPPUNIT_ASSERT_EQUAL(R,Rout));
00463 }
00464
00465 }
00466
00467 void FramesTest::TestQuaternion() {
00468 Rotation R;
00469 Rotation R2;
00470 double x,y,z,w;
00471 double x2,y2,z2,w2;
00472
00473
00474 R.GetQuaternion(x,y,z,w);
00475 R2.Quaternion(x,y,z,w);
00476 CPPUNIT_ASSERT_EQUAL(R,R2);
00477
00478
00479 R = Rotation::EulerZYX(0,0,45*deg2rad);
00480 R.GetQuaternion(x,y,z,w);
00481 CPPUNIT_ASSERT_DOUBLES_EQUAL(x, sin((45*deg2rad)/2), epsilon);
00482 CPPUNIT_ASSERT_DOUBLES_EQUAL(y, 0, epsilon);
00483 CPPUNIT_ASSERT_DOUBLES_EQUAL(z, 0, epsilon);
00484 CPPUNIT_ASSERT_DOUBLES_EQUAL(w, cos((45*deg2rad)/2), epsilon);
00485 R2 = Rotation::Quaternion(x,y,z,w);
00486 CPPUNIT_ASSERT_EQUAL(R,R2);
00487
00488
00489 R2 = Rotation::Quaternion(sin((45*deg2rad)/2), 0, 0, cos((45*deg2rad)/2));
00490 CPPUNIT_ASSERT_EQUAL(R,R2);
00491 R2.GetQuaternion(x2,y2,z2,w2);
00492 CPPUNIT_ASSERT_DOUBLES_EQUAL(x, x2, epsilon);
00493 CPPUNIT_ASSERT_DOUBLES_EQUAL(y, y2, epsilon);
00494 CPPUNIT_ASSERT_DOUBLES_EQUAL(z, z2, epsilon);
00495 CPPUNIT_ASSERT_DOUBLES_EQUAL(w, w2, epsilon);
00496
00497
00498 R = Rotation::EulerZYX(45*deg2rad,0,45*deg2rad);
00499 R.GetQuaternion(x,y,z,w);
00500 R2 = Rotation::Quaternion(x,y,z,w);
00501 CPPUNIT_ASSERT_EQUAL(R,R2);
00502 R2.GetQuaternion(x2,y2,z2,w2);
00503 CPPUNIT_ASSERT_DOUBLES_EQUAL(x, x2, epsilon);
00504 CPPUNIT_ASSERT_DOUBLES_EQUAL(y, y2, epsilon);
00505 CPPUNIT_ASSERT_DOUBLES_EQUAL(z, z2, epsilon);
00506 CPPUNIT_ASSERT_DOUBLES_EQUAL(w, w2, epsilon);
00507 }
00508
00509
00510 void FramesTest::TestRotationDiff() {
00511
00512 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(0*deg2rad)), Vector(0,0,0));
00513 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(90*deg2rad)), Vector(M_PI/2,0,0));
00514 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(180*deg2rad)), Vector(M_PI,0,0));
00515 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(270*deg2rad)), Vector(-M_PI/2,0,0));
00516 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(360*deg2rad)), Vector(0,0,0));
00517 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-360*deg2rad)), Vector(0,0,0));
00518 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-270*deg2rad)), Vector(M_PI/2,0,0));
00519 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-180*deg2rad)), Vector(M_PI,0,0));
00520 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-90*deg2rad)), Vector(-M_PI/2,0,0));
00521 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-0*deg2rad)), Vector(0,0,0));
00522
00523 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(0*deg2rad)), Vector(0,0,0));
00524 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(90*deg2rad)), Vector(0,M_PI/2,0));
00525 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(180*deg2rad)), Vector(0,M_PI,0));
00526 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(270*deg2rad)), Vector(0,-M_PI/2,0));
00527 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(360*deg2rad)), Vector(0,0,0));
00528 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-360*deg2rad)), Vector(0,0,0));
00529 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-270*deg2rad)), Vector(0,M_PI/2,0));
00530 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-180*deg2rad)), Vector(0,M_PI,0));
00531 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-90*deg2rad)), Vector(0,-M_PI/2,0));
00532 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-0*deg2rad)), Vector(0,0,0));
00533
00534 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(0*deg2rad)), Vector(0,0,0));
00535 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(90*deg2rad)), Vector(0,0,M_PI/2));
00536 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(180*deg2rad)), Vector(0,0,M_PI));
00537 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(270*deg2rad)), Vector(0,0,-M_PI/2));
00538 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(360*deg2rad)), Vector(0,0,0));
00539 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-360*deg2rad)), Vector(0,0,0));
00540 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-270*deg2rad)), Vector(0,0,M_PI/2));
00541 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-180*deg2rad)), Vector(0,0,M_PI));
00542 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-90*deg2rad)), Vector(0,0,-M_PI/2));
00543 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-0*deg2rad)), Vector(0,0,0));
00544
00545 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotZ(90*deg2rad)), Vector(0,0,M_PI/2));
00546 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotY(90*deg2rad)), Vector(0,M_PI/2,0));
00547 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotZ(90*deg2rad)), Vector(0,0,M_PI/2));
00548
00549 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::Identity(), Rotation::RotX(90*deg2rad)), Vector(M_PI/2,0,0));
00550 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::Identity(), Rotation::RotY(90*deg2rad)), Vector(0,M_PI/2,0));
00551 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::Identity(), Rotation::RotZ(90*deg2rad)), Vector(0,0,M_PI/2));
00552
00553 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RPY(+0*deg2rad,0,-90*deg2rad),
00554 Rotation::RPY(-0*deg2rad,0,+90*deg2rad)),
00555 Vector(0,0,M_PI));
00556 CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RPY(+5*deg2rad,0,-0*deg2rad),
00557 Rotation::RPY(-5*deg2rad,0,+0*deg2rad)),
00558 Vector(-10*deg2rad,0,0));
00559
00560 KDL::Rotation R1 = Rotation::RPY(+5*deg2rad,0,-90*deg2rad);
00561 CPPUNIT_ASSERT_EQUAL(KDL::diff(R1, Rotation::RPY(-5*deg2rad,0,+90*deg2rad)),
00562 R1*Vector(0, 0, 180*deg2rad));
00563 }
00564
00565 void FramesTest::TestFrame() {
00566 Vector v(3,4,5);
00567 Wrench w(Vector(7,-1,3),Vector(2,-3,3)) ;
00568 Twist t(Vector(6,3,5),Vector(4,-2,7)) ;
00569 Rotation R ;
00570 Frame F;
00571 Frame F2 ;
00572 F = Frame(Rotation::EulerZYX(10*deg2rad,20*deg2rad,-10*deg2rad),Vector(4,-2,1));
00573 F2=F ;
00574 CPPUNIT_ASSERT_EQUAL(F,F2);
00575 CPPUNIT_ASSERT_EQUAL(F.Inverse(F*v),v);
00576 CPPUNIT_ASSERT_EQUAL(F.Inverse(F*t),t);
00577 CPPUNIT_ASSERT_EQUAL(F.Inverse(F*w),w);
00578 CPPUNIT_ASSERT_EQUAL(F*F.Inverse(v),v);
00579 CPPUNIT_ASSERT_EQUAL(F*F.Inverse(t),t);
00580 CPPUNIT_ASSERT_EQUAL(F*F.Inverse(w),w);
00581 CPPUNIT_ASSERT_EQUAL(F*Frame::Identity(),F);
00582 CPPUNIT_ASSERT_EQUAL(Frame::Identity()*F,F);
00583 CPPUNIT_ASSERT_EQUAL(F*(F*(F*v)),(F*F*F)*v);
00584 CPPUNIT_ASSERT_EQUAL(F*(F*(F*t)),(F*F*F)*t);
00585 CPPUNIT_ASSERT_EQUAL(F*(F*(F*w)),(F*F*F)*w);
00586 CPPUNIT_ASSERT_EQUAL(F*F.Inverse(),Frame::Identity());
00587 CPPUNIT_ASSERT_EQUAL(F.Inverse()*F,Frame::Identity());
00588 CPPUNIT_ASSERT_EQUAL(F.Inverse()*v,F.Inverse(v));
00589 }
00590
00591 void FramesTest::TestJntArray()
00592 {
00593 JntArray a1(4);
00594 random(a1(0));
00595 random(a1(1));
00596 random(a1(2));
00597 random(a1(3));
00598 JntArray a2(a1);
00599 CPPUNIT_ASSERT(Equal(a2,a1));
00600
00601 SetToZero(a2);
00602 CPPUNIT_ASSERT(!Equal(a1,a2));
00603
00604 JntArray a3(4);
00605 CPPUNIT_ASSERT(Equal(a2,a3));
00606
00607 a1=a2;
00608 CPPUNIT_ASSERT(Equal(a1,a3));
00609
00610 random(a1(0));
00611 random(a1(1));
00612 random(a1(2));
00613 random(a1(3));
00614
00615 Add(a1,a2,a3);
00616 CPPUNIT_ASSERT(Equal(a1,a3));
00617
00618 random(a2(0));
00619 random(a2(1));
00620 random(a2(2));
00621 random(a2(3));
00622 Add(a1,a2,a3);
00623 Subtract(a3,a2,a3);
00624 CPPUNIT_ASSERT(Equal(a1,a3));
00625
00626 Multiply(a1,2,a3);
00627 Add(a1,a1,a2);
00628 CPPUNIT_ASSERT(Equal(a2,a3));
00629
00630 double a;
00631 random(a);
00632 Multiply(a1,a,a3);
00633 Divide(a3,a,a2);
00634 CPPUNIT_ASSERT(Equal(a2,a1));
00635 }
00636
00637
00638 void FramesTest::TestJntArrayWhenEmpty()
00639 {
00640 JntArray a1;
00641 JntArray a2;
00642 JntArray a3(a2);
00643
00644
00645 CPPUNIT_ASSERT_EQUAL((unsigned int)0,a1.rows());
00646 CPPUNIT_ASSERT(Equal(a2,a1));
00647
00648 a2 = a1;
00649 CPPUNIT_ASSERT(Equal(a2,a1));
00650
00651 SetToZero(a2);
00652 CPPUNIT_ASSERT(Equal(a2,a1));
00653
00654 Add(a1,a2,a3);
00655 CPPUNIT_ASSERT(Equal(a1,a3));
00656
00657 Subtract(a1,a2,a3);
00658 CPPUNIT_ASSERT(Equal(a1,a3));
00659
00660 Multiply(a1,3.1,a3);
00661 CPPUNIT_ASSERT(Equal(a1,a3));
00662
00663 Divide(a1,3.1,a3);
00664 CPPUNIT_ASSERT(Equal(a1,a3));
00665
00666
00667
00668
00669
00670
00671
00672
00673
00674 a1.resize(3);
00675 a2.resize(3);
00676 CPPUNIT_ASSERT_EQUAL((unsigned int)3,a1.rows());
00677 CPPUNIT_ASSERT(Equal(a2,a1));
00678
00679 random(a1(0));
00680 random(a1(1));
00681 random(a1(2));
00682 a1 = a2;
00683 CPPUNIT_ASSERT(Equal(a1,a2));
00684 CPPUNIT_ASSERT_EQUAL(a1(1),a2(1));
00685
00686 a3.resize(3);
00687 Subtract(a1,a2,a3);
00688 SetToZero(a1);
00689 CPPUNIT_ASSERT(Equal(a1,a3));
00690 }
00691
00692