jacobianframetests.cpp
Go to the documentation of this file.
00001 #include <kdl/frames.hpp>
00002 #include <kdl/frames_io.hpp>
00003 #include <kdl/framevel.hpp>
00004 #include <kdl/framevel_io.hpp>
00005 #include <kdl/jacobianexpr.hpp>
00006 #include <kdl/jacobianframe.hpp>
00007 #include <kdl/jacobianframevel.hpp>
00008 #include "jacobianframetests.hpp"
00009 
00010 namespace KDL {
00011 
00012 
00013 void checkDiffs() {
00014     KDL_CTX;
00015         double adouble,bdouble;
00016         Vector avector,bvector;
00017         Twist  atwist,btwist;
00018         Rotation arot,brot;
00019         Frame  aframe,bframe;
00020         // large deviations :
00021         random(adouble);random(bdouble);
00022         random(avector);random(bvector);
00023         random(atwist);random(btwist);
00024         random(arot);random(brot);
00025         random(aframe);random(bframe);
00026         double dt=0.1;
00027         double eps = 1E-10;
00028         checkEqual(bdouble,addDelta(adouble,diff(adouble,bdouble,dt),dt),eps);
00029         checkEqual(bvector,addDelta(avector,diff(avector,bvector,dt),dt),eps);
00030         checkEqual(btwist,addDelta(atwist,diff(atwist,btwist,dt),dt),eps);
00031         checkEqual(brot,addDelta(arot,diff(arot,brot,dt),dt),eps);
00032         checkEqual(bframe,addDelta(aframe,diff(aframe,bframe,dt),dt),eps);
00033 
00034         // small deviations
00035         dt = 0.00001;
00036         double ddouble;
00037         Vector dvector;
00038         Twist  dtwist;    
00039         Vector drot;    // there is no error in the naming ...
00040         Twist  dframe;
00041         random(ddouble);random(dvector);random(dtwist);random(drot);random(dframe);
00042         checkEqual(ddouble,diff(adouble,addDelta(adouble,ddouble,dt),dt),eps);
00043         checkEqual(dvector,diff(avector,addDelta(avector,dvector,dt),dt),eps);
00044         checkEqual(dtwist,diff(atwist,addDelta(atwist,dtwist,dt),dt),eps);
00045         checkEqual(drot,diff(arot,addDelta(arot,drot,dt),dt),eps);
00046         checkEqual(dframe,diff(aframe,addDelta(aframe,dframe,dt),dt),eps);
00047 }
00048 
00049 
00050 void checkEulerZYX() {
00051         // Take care of the order of the arguments :
00052     KDL_CTX;
00053         int nrofcol=3;
00054         Jacobian<Rotation> R(nrofcol);
00055         Jacobian<Rotation> R2(nrofcol);
00056         Jacobian<double> alpha(nrofcol);
00057         random(alpha);
00058         Jacobian<double> beta(nrofcol);
00059         random(beta);
00060         Jacobian<double> gamma(nrofcol);
00061         random(gamma);
00062         SetEulerZYX(gamma,beta,alpha,R);        
00063         // now we have a random frame R.
00064         int result=GetEulerZYX(R,gamma,beta,alpha);
00065         assert( result == 0);
00066         SetEulerZYX(gamma,beta,alpha,R2);       
00067         checkEqual(R.value(),R2.value(),0.0001);
00068         for (int i=0;i<nrofcol;i++) {
00069                 checkEqual(R.deriv(i),R2.deriv(i),0.0001);
00070         }
00071         double dt= 1E-8;
00072         double eps = 1E-4;
00073         std::cout << "Tests with numerical derivatives for EulerZYX " << std::endl;
00074         for (int i=0;i<nrofcol;i++) {
00075                 Rotation R2 = Rotation::EulerZYX(alpha.value(),beta.value(),gamma.value());
00076                 checkEqual( R.deriv(i), 
00077                                         diff(
00078                                                 R.value(), 
00079                                                 Rotation::EulerZYX(
00080                                                         alpha.value()+ alpha.deriv(i)*dt, 
00081                                                         beta.value() + beta.deriv(i)*dt, 
00082                                                         gamma.value()+ gamma.deriv(i)*dt),
00083                                                 dt),
00084                                         eps);
00085         }               
00086 }
00087 
00088 
00089 void checkFrameOps() {
00090     KDL_CTX;
00091         checkDiffs();
00092 
00093         checkUnary<OpInverse,Frame>::check();
00094         checkUnary<OpNegate,Vector>::check();
00095         checkUnary<OpNorm,Vector>::check();
00096     checkUnary<OpRotX,double>::check();
00097     checkUnary<OpRotY,double>::check();
00098     checkUnary<OpRotZ,double>::check();
00099     checkUnary<OpUnitX,Rotation>::check();
00100     checkUnary<OpUnitY,Rotation>::check();
00101     checkUnary<OpUnitZ,Rotation>::check();
00102     checkUnary<OpInverse,Rotation>::check();
00103     checkUnary<OpNegate,Twist>::check();
00104 
00105         checkBinary<OpMult,Frame,Frame>::check();
00106         checkBinary<OpMult,Frame,Vector>::check();
00107         checkBinary<OpDot,Vector,Vector>::check();
00108         checkBinary<OpMult,Vector,Vector>::check();
00109         checkBinary<OpAdd,Vector,Vector>::check();
00110         checkBinary<OpSub,Vector,Vector>::check();
00111     checkBinary<OpMult,Rotation,Rotation>::check();
00112     checkBinary<OpMult,Rotation,Vector>::check();    
00113     checkBinary<OpMult,Rotation,Twist>::check();    
00114     checkBinary<OpAdd,Twist,Twist>::check();    
00115     checkBinary<OpSub,Twist,Twist>::check();    
00116     checkBinary<OpMult,Twist,double>::check();    
00117     checkBinary<OpMult,double,Twist>::check();    
00118     checkBinary<OpRefPoint,Twist,Vector>::check();
00119     checkBinary<OpAdd,Wrench,Wrench>::check();    
00120     checkBinary<OpSub,Wrench,Wrench>::check();    
00121     checkBinary<OpMult,Wrench,double>::check();    
00122     checkBinary<OpMult,double,Wrench>::check();    
00123     checkBinary<OpRefPoint,Wrench,Vector>::check();     
00124         checkBinary<OpDiff,Vector,Vector>::check();
00125 
00126         //checkBinary_displ<OpDiff,Rotation,Rotation>::check(); 
00127 
00128 
00129         checkEulerZYX();
00130 }
00131 
00132 void checkFrameVelOps() {
00133     KDL_CTX;
00134         checkDiffs();
00135 
00136         checkUnaryVel<OpNegate,VectorVel>::check();
00137         checkUnaryVel<OpNorm,VectorVel>::check();
00138         checkUnaryVel<OpInverse,FrameVel>::check();
00139         checkUnaryVel<OpRotation,FrameVel>::check();
00140         checkUnaryVel<OpOrigin,FrameVel>::check();
00141         checkUnaryVel<OpUnitX,RotationVel>::check();
00142         checkUnaryVel<OpUnitY,RotationVel>::check();
00143         checkUnaryVel<OpUnitZ,RotationVel>::check();
00144         checkUnaryVel<OpCoordX,VectorVel>::check();
00145         checkUnaryVel<OpCoordY,VectorVel>::check();
00146         checkUnaryVel<OpCoordZ,VectorVel>::check();
00147         checkUnaryVel<OpRotation,FrameVel>::check();
00148 
00149     //checkUnary<OpRotX,double>::check();
00150     //checkUnary<OpRotY,double>::check();
00151     //checkUnary<OpRotZ,double>::check();
00152     checkUnaryVel<OpInverse,RotationVel>::check();
00153     checkUnaryVel<OpNegate,TwistVel>::check();
00154 
00155         checkBinaryVel<OpMult,FrameVel,FrameVel>::check();
00156         checkBinaryVel<OpMult,FrameVel,VectorVel>::check();
00157         checkBinaryVel<OpDot,VectorVel,VectorVel>::check();
00158         checkBinaryVel<OpMult,VectorVel,VectorVel>::check();
00159         checkBinaryVel<OpAdd,VectorVel,VectorVel>::check();
00160         checkBinaryVel<OpSub,VectorVel,VectorVel>::check();
00161         checkBinaryVel<OpMult,doubleVel,VectorVel>::check();
00162         checkBinaryVel<OpMult,VectorVel,doubleVel>::check();
00163     checkBinaryVel<OpMult,RotationVel,RotationVel>::check();
00164     checkBinaryVel<OpMult,RotationVel,VectorVel>::check();    
00165     checkBinaryVel<OpMult,RotationVel,TwistVel>::check();    
00166     checkBinaryVel<OpAdd,TwistVel,TwistVel>::check();    
00167     checkBinaryVel<OpSub,TwistVel,TwistVel>::check();    
00168     checkBinaryVel<OpMult,TwistVel,doubleVel>::check();    
00169     checkBinaryVel<OpMult,doubleVel,TwistVel>::check(); 
00170     checkBinaryVel<OpRefPoint,TwistVel,VectorVel>::check();
00171     /*
00172     checkBinary<OpAdd,Wrench,Wrench>::check();    
00173     checkBinary<OpSub,Wrench,Wrench>::check();    
00174     checkBinary<OpMult,Wrench,double>::check();    
00175     checkBinary<OpMult,double,Wrench>::check();    
00176     checkBinary<OpRefPoint,Wrench,Vector>::check();     */
00177         checkBinaryVel<OpDiff,VectorVel,VectorVel>::check();
00178         //checkBinaryVel<OpDiff,RotationVel,RotationVel>::check(); WHY ? 
00179         //checkBinaryVel<OpDiff,FrameVel,FrameVel>::check();  WHY ?
00180         
00181 
00182         //checkEulerZYX();
00183 }
00184 
00185 
00186 } // namespace ORO_Geometry
00187 
00188 
00189 
00190 


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