framestest.cpp
Go to the documentation of this file.
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         // Wrench
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         // Wrench
00096         Wrench   w(Vector(7,-1,3),Vector(2,-3,3));
00097         Twist    t(Vector(6,3,5),Vector(4,-2,7));
00098         // Rotation             
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 // compare a rotation R with the expected angle and axis
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         // if beta== +/- M_PI/2 => multiple solutions available, gamma will be choosen to be zero !
00232         // so we test with gamma==0 !
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         //TESTEULERZYX(0.1,-M_PI/2,0.3)
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         // extremes of the range:
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         //TESTEULERZYX(0,-M_PI/2,M_PI)  gamma will be chosen zero
00255         //TESTEULERZYX(0,M_PI/2,-M_PI)  gamma will be chosen zero
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));         // no rotation
00293         TestArbitraryRotation(msg, v, 45,   45, expectedVector);
00294         TestArbitraryRotation(msg, v, 90,   90, expectedVector);
00295         TestArbitraryRotation(msg, v, 179, 179, expectedVector);
00296 //      TestArbitraryRotation(msg, v, 180, 180, expectedVector);        // sign VARIES by case because 180 degrees not
00297                                                                 // full determined: it can return +/- the axis
00298                                                                 // depending on what the original axis was.
00299                                                                 // BOTH RESULTS ARE CORRECT.
00300         TestArbitraryRotation(msg, v, 181, 179, -expectedVector);       // >+180 rotation => <+180 + negative axis
00301         TestArbitraryRotation(msg, v, 270,  90, -expectedVector);
00302         TestArbitraryRotation(msg, v, 359,   1, -expectedVector);
00303         TestArbitraryRotation(msg, v, 360,   0, Vector(0,0,1));         // no rotation
00304         TestArbitraryRotation(msg, v, 361,   1, expectedVector);
00305         TestArbitraryRotation(msg, v, 450,  90, expectedVector);
00306         TestArbitraryRotation(msg, v, 539, 179, expectedVector);
00307 //      TestArbitraryRotation(msg, v, 540, 180, expectedVector);        // see above 
00308         TestArbitraryRotation(msg, v, 541, 179, -expectedVector);       // like 181
00309         TestArbitraryRotation(msg, v, 630,  90, -expectedVector);       // like 270
00310         TestArbitraryRotation(msg, v, 719,   1, -expectedVector);       // like 259
00311         TestArbitraryRotation(msg, v, 720,   0, Vector(0,0,1));         // no rotation
00312 
00313         TestArbitraryRotation(msg, v, -45,   45, -expectedVector);
00314         TestArbitraryRotation(msg, v, -90,   90, -expectedVector);
00315         TestArbitraryRotation(msg, v, -179, 179, -expectedVector);
00316 //      TestArbitraryRotation(msg, v, -180, 180, expectedVector);       // see above 
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));        // no rotation
00321         TestArbitraryRotation(msg, v, -361,   1, -expectedVector);
00322         TestArbitraryRotation(msg, v, -450,  90, -expectedVector);
00323         TestArbitraryRotation(msg, v, -539, 179, -expectedVector);
00324 //      TestArbitraryRotation(msg, v, -540, 180, -expectedVector);      // see above 
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));        // no rotation
00329 }
00330 
00331 void FramesTest::TestRotation() {
00332         TestRotation2(Vector(3,4,5),10*deg2rad,20*deg2rad,30*deg2rad);
00333 
00334         // X axis
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         // Y axis
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         // Z axis
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         // X,Y axes
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         // X,Z axes
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         // Y,Z axes
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         // X,Y,Z axes
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         // these change ... some of the -180 are the same as the +180, and some
00357         // results are the opposite sign.
00358         TestOneRotation("rot([1,0,0],180)", KDL::Rotation::Rot(KDL::Vector(1,0,0),180*deg2rad), 180*deg2rad, Vector(1,0,0));
00359         // same as +180
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         // same as +180
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         // same as +180
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         // same as +180
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         // same as +180
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         // opposite of +180
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         // opposite of +180
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         // same as +180
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         // same as +180
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         // same as +180
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         // same as +180
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     // identity R -> quat -> R2
00405     R.GetQuaternion(x,y,z,w);
00406     R2.Quaternion(x,y,z,w);
00407         CPPUNIT_ASSERT_EQUAL(R,R2);
00408 
00409     // 45 deg rotation in X
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     // direct 45 deg rotation in X
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     // 45 deg rotation in X and in Z
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));           // no rotation
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));         // no rotation
00448         CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-360*deg2rad)), Vector(0,0,0));        // no rotation
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));          // no rotation
00453 
00454         CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(0*deg2rad)), Vector(0,0,0));           // no rotation
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));         // no rotation
00459         CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-360*deg2rad)), Vector(0,0,0));        // no rotation
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));          // no rotation
00464 
00465         CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(0*deg2rad)), Vector(0,0,0));           // no rotation
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));         // no rotation
00470         CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-360*deg2rad)), Vector(0,0,0));        // no rotation
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));          // no rotation
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         // won't assert()
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         // MultiplyJacobian() - not tested here
00598     
00599     
00600         /* will assert() - not tested here
00601        double j1 = a1(0);
00602         */      
00603     
00604         // and now resize, and do just a few tests
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); // a3 = a2 - a1 = {0}
00619     SetToZero(a1);
00620     CPPUNIT_ASSERT(Equal(a1,a3));
00621 }
00622 
00623 


orocos_kdl
Author(s):
autogenerated on Wed Aug 26 2015 15:14:14