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::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         // Wrench
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         // Wrench
00106         Wrench   w(Vector(7,-1,3),Vector(2,-3,3));
00107         Twist    t(Vector(6,3,5),Vector(4,-2,7));
00108         // Rotation             
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 // compare a rotation R with the expected angle and axis
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         // if beta== +/- M_PI/2 => multiple solutions available, gamma will be chosen to be zero !
00242         // so we test with gamma==0 !
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         //TESTEULERZYX(0.1,-M_PI/2,0.3)
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         // extremes of the range:
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         //TESTEULERZYX(0,-M_PI/2,M_PI)  gamma will be chosen zero
00265         //TESTEULERZYX(0,M_PI/2,-M_PI)  gamma will be chosen zero
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));         // no rotation
00303         TestArbitraryRotation(msg, v, 45,   45, expectedVector);
00304         TestArbitraryRotation(msg, v, 90,   90, expectedVector);
00305         TestArbitraryRotation(msg, v, 179, 179, expectedVector);
00306 //      TestArbitraryRotation(msg, v, 180, 180, expectedVector);        // sign VARIES by case because 180 degrees not
00307                                                                 // full determined: it can return +/- the axis
00308                                                                 // depending on what the original axis was.
00309                                                                 // BOTH RESULTS ARE CORRECT.
00310         TestArbitraryRotation(msg, v, 181, 179, -expectedVector);       // >+180 rotation => <+180 + negative axis
00311         TestArbitraryRotation(msg, v, 270,  90, -expectedVector);
00312         TestArbitraryRotation(msg, v, 359,   1, -expectedVector);
00313         TestArbitraryRotation(msg, v, 360,   0, Vector(0,0,1));         // no rotation
00314         TestArbitraryRotation(msg, v, 361,   1, expectedVector);
00315         TestArbitraryRotation(msg, v, 450,  90, expectedVector);
00316         TestArbitraryRotation(msg, v, 539, 179, expectedVector);
00317 //      TestArbitraryRotation(msg, v, 540, 180, expectedVector);        // see above 
00318         TestArbitraryRotation(msg, v, 541, 179, -expectedVector);       // like 181
00319         TestArbitraryRotation(msg, v, 630,  90, -expectedVector);       // like 270
00320         TestArbitraryRotation(msg, v, 719,   1, -expectedVector);       // like 259
00321         TestArbitraryRotation(msg, v, 720,   0, Vector(0,0,1));         // no rotation
00322 
00323         TestArbitraryRotation(msg, v, -45,   45, -expectedVector);
00324         TestArbitraryRotation(msg, v, -90,   90, -expectedVector);
00325         TestArbitraryRotation(msg, v, -179, 179, -expectedVector);
00326 //      TestArbitraryRotation(msg, v, -180, 180, expectedVector);       // see above 
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));        // no rotation
00331         TestArbitraryRotation(msg, v, -361,   1, -expectedVector);
00332         TestArbitraryRotation(msg, v, -450,  90, -expectedVector);
00333         TestArbitraryRotation(msg, v, -539, 179, -expectedVector);
00334 //      TestArbitraryRotation(msg, v, -540, 180, -expectedVector);      // see above 
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));        // no rotation
00339 }
00340 
00341 void FramesTest::TestRotation() {
00342         TestRotation2(Vector(3,4,5),10*deg2rad,20*deg2rad,30*deg2rad);
00343 
00344         // X axis
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         // Y axis
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         // Z axis
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         // X,Y axes
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         // X,Z axes
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         // Y,Z axes
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         // X,Y,Z axes
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         // these change ... some of the -180 are the same as the +180, and some
00367         // results are the opposite sign.
00368         TestOneRotation("rot([1,0,0],180)", KDL::Rotation::Rot(KDL::Vector(1,0,0),180*deg2rad), 180*deg2rad, Vector(1,0,0));
00369         // same as +180
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         // same as +180
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         // same as +180
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         // same as +180
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         // same as +180
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         // opposite of +180
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         // opposite of +180
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         // same as +180
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         // same as +180
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         // same as +180
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         // same as +180
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     // rpy -> rotation
00417     KDL::Rotation kdlRotation1 = KDL::Rotation::RPY(roll, pitch, yaw);
00418 
00419     // rotation -> angle-axis (with KDL::GetRotAngle)
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   // Test GetRotAngle on slightly non-orthogonal rotation matrices
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   // Tests to show that GetRotAngle does not work for an improper rotation matrix which has a determinant of -1;
00443   // an improper rotation matrix corresponds to a rotation between a right-hand and left-hand coordinate system
00444   {
00445     Vector axis;
00446     double angle;
00447     Rotation R, Rout;
00448     double det;
00449     // Improper Rotation Matrix for 120 deg rotation
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     // Improper Rotation matrix for 180 deg rotation (singular)
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     // identity R -> quat -> R2
00474     R.GetQuaternion(x,y,z,w);
00475     R2.Quaternion(x,y,z,w);
00476         CPPUNIT_ASSERT_EQUAL(R,R2);
00477 
00478     // 45 deg rotation in X
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     // direct 45 deg rotation in X
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     // 45 deg rotation in X and in Z
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));           // no rotation
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));         // no rotation
00517         CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotX(0*deg2rad), Rotation::RotX(-360*deg2rad)), Vector(0,0,0));        // no rotation
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));          // no rotation
00522 
00523         CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(0*deg2rad)), Vector(0,0,0));           // no rotation
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));         // no rotation
00528         CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotY(0*deg2rad), Rotation::RotY(-360*deg2rad)), Vector(0,0,0));        // no rotation
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));          // no rotation
00533 
00534         CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(0*deg2rad)), Vector(0,0,0));           // no rotation
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));         // no rotation
00539         CPPUNIT_ASSERT_EQUAL(KDL::diff(Rotation::RotZ(0*deg2rad), Rotation::RotZ(-360*deg2rad)), Vector(0,0,0));        // no rotation
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));          // no rotation
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         // won't assert()
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         // MultiplyJacobian() - not tested here
00667     
00668     
00669         /* will assert() - not tested here
00670        double j1 = a1(0);
00671         */      
00672     
00673         // and now resize, and do just a few tests
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); // a3 = a2 - a1 = {0}
00688     SetToZero(a1);
00689     CPPUNIT_ASSERT(Equal(a1,a3));
00690 }
00691 
00692 


orocos_kdl
Author(s):
autogenerated on Fri Jun 14 2019 19:33:22