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
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
00035 dt = 0.00001;
00036 double ddouble;
00037 Vector dvector;
00038 Twist dtwist;
00039 Vector drot;
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
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
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
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
00150
00151
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
00173
00174
00175
00176
00177 checkBinaryVel<OpDiff,VectorVel,VectorVel>::check();
00178
00179
00180
00181
00182
00183 }
00184
00185
00186 }
00187
00188
00189
00190