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