$search
00001 #ifndef PVTESTS_H 00002 #define PVTESTS_H 00003 00004 00005 00006 #include <kdl/jacobianexpr.hpp> 00007 #include <kdl/test_macros.h> 00008 #include <iostream> 00009 00010 #include <string> 00011 #include <iomanip> 00012 00013 namespace KDL { 00014 00015 template <typename T> 00016 void random(Jacobian<T>& rv) { 00017 random(rv.value()); 00018 for (int i=0;i<rv.nrOfDeriv();++i) { 00019 random(rv.deriv(i)); 00020 } 00021 } 00022 00023 00024 template <typename T> 00025 void posrandom(Jacobian<T>& rv) { 00026 posrandom(rv.value()); 00027 for (int i=0;i<rv.nrOfDeriv();++i) { 00028 posrandom(rv.deriv(i)); 00029 } 00030 } 00031 00032 00033 template <typename T> 00034 inline void checkEqual(const T& a,const T& b,double eps) { 00035 KDL_CTX; 00036 KDL_DIFF(a,b); 00037 assert(Equal(a,b,eps)); 00038 } 00039 00040 00041 template <typename OpID,typename A> 00042 class checkUnary { 00043 typedef UnaryOp<OpID,A> myOp; 00044 public: 00045 inline static void check(void (*rnd)(Jacobian<A>&) = &random,double dt=1E-8,double eps=1E-4,int size=1) { 00046 KDL_CTX; 00047 Jacobian<A> a(size); 00048 rnd(a); 00049 KDL_ARG1(a); 00050 int i; 00051 for (i=0;i<a.nrOfDeriv();++i) { 00052 checkEqual( 00053 myOp::deriv(a.value(),a.deriv(i)), 00054 diff( 00055 myOp::value(a.value()), 00056 myOp::value( 00057 addDelta(a.value(),a.deriv(i),dt) 00058 ), 00059 dt), 00060 eps 00061 ); 00062 } 00063 } 00064 }; 00065 00066 00067 template <typename OpID,typename A> 00068 class checkUnaryVel { 00069 typedef UnaryOp<OpID,A> myOp; 00070 00071 public: 00072 inline static void check(void (*rnd)(Jacobian<A>&) = &random,double dt=1E-8,double eps=1E-4,int size=1) { 00073 KDL_CTX; 00074 Jacobian<A> a(size); 00075 rnd(a); 00076 KDL_ARG1(a); 00077 int i; 00078 for (i=0;i<a.nrOfDeriv();++i) { 00079 KDL_MSG("testing value() components of deriv "); 00080 checkEqual( 00081 myOp::deriv(a.value(),a.deriv(i)).value(), 00082 diff( 00083 myOp::value(a.value()), 00084 myOp::value( 00085 addDelta(a.value(),a.deriv(i).value(),dt) 00086 ), 00087 dt), 00088 eps 00089 ); 00090 typename Traits<A>::derivType d1( 00091 addDelta(a.deriv(i).value(), a.deriv(i).deriv(),dt)); 00092 typename Traits<A>::valueType a1( 00093 addDelta(a.value(),a.deriv(i).value(),dt) 00094 ); 00095 KDL_MSG("testing deriv() components of deriv "); 00096 checkEqual( 00097 myOp::deriv(a.value(),a.deriv(i)).deriv(), 00098 diff( 00099 myOp::deriv(a.value(),a.deriv(i)).value(), 00100 myOp::deriv(a1, d1).value(), 00101 dt), 00102 eps 00103 ); 00104 } 00105 } 00106 }; 00107 00108 template <typename OpID,typename A,typename B> 00109 class checkBinary { 00110 typedef BinaryOp<OpID,A,B> myOp; 00111 public: 00112 inline static void check(double dt=1E-8,double eps=1E-4,int size=1) { 00113 KDL_CTX; 00114 Jacobian<A> a(size); 00115 random(a); 00116 Jacobian<B> b(size); 00117 random(b); 00118 KDL_ARG2(a,b); 00119 int i; 00120 for (i=0;i<a.nrOfDeriv();++i) { 00121 checkEqual( 00122 myOp::derivVV(a.value(),a.deriv(i),b.value(),b.deriv(i)), 00123 diff( 00124 myOp::value(a.value(),b.value()), 00125 myOp::value( 00126 addDelta(a.value(),a.deriv(i),dt), 00127 addDelta(b.value(),b.deriv(i),dt) 00128 ), 00129 dt), 00130 eps 00131 ); 00132 checkEqual( 00133 myOp::derivVC(a.value(),a.deriv(i),b.value()), 00134 diff( 00135 myOp::value(a.value(),b.value()), 00136 myOp::value( 00137 addDelta(a.value(),a.deriv(i),dt), 00138 b.value() 00139 ), 00140 dt), 00141 eps 00142 ); 00143 checkEqual( 00144 myOp::derivCV(a.value(),b.value(),b.deriv(i)), 00145 diff( 00146 myOp::value(a.value(),b.value()), 00147 myOp::value( 00148 a.value(), 00149 addDelta(b.value(),b.deriv(i),dt) 00150 ), 00151 dt), 00152 eps 00153 ); 00154 00155 } 00156 } 00157 }; 00158 00159 00160 template <typename OpID,typename A,typename B> 00161 class checkBinary_displ { 00162 typedef BinaryOp<OpID,A,B> myOp; 00163 public: 00164 inline static void check(double dt=1E-8,double eps=1E-4,int size=1) { 00165 KDL_CTX; 00166 Jacobian<A> a(size); 00167 random(a); 00168 Jacobian<B> b(size); 00169 random(b); 00170 KDL_ARG2(a,b); 00171 int i; 00172 for (i=0;i<a.nrOfDeriv();++i) { 00173 checkEqual( 00174 myOp::derivVV(a.value(),a.deriv(i),b.value(),b.deriv(i)), 00175 diff_displ( 00176 myOp::value(a.value(),b.value()), 00177 myOp::value( 00178 addDelta(a.value(),a.deriv(i),dt), 00179 addDelta(b.value(),b.deriv(i),dt) 00180 ), 00181 dt), 00182 eps 00183 ); 00184 checkEqual( 00185 myOp::derivVC(a.value(),a.deriv(i),b.value()), 00186 diff_displ( 00187 myOp::value(a.value(),b.value()), 00188 myOp::value( 00189 addDelta(a.value(),a.deriv(i),dt), 00190 b.value() 00191 ), 00192 dt), 00193 eps 00194 ); 00195 checkEqual( 00196 myOp::derivCV(a.value(),b.value(),b.deriv(i)), 00197 diff_displ( 00198 myOp::value(a.value(),b.value()), 00199 myOp::value( 00200 a.value(), 00201 addDelta(b.value(),b.deriv(i),dt) 00202 ), 00203 dt), 00204 eps 00205 ); 00206 00207 } 00208 } 00209 }; 00210 00211 00212 template <typename OpID,typename A,typename B> 00213 class checkBinaryVel { 00214 typedef BinaryOp<OpID,A,B> myOp; 00215 public: 00216 inline static void check(double dt=1E-8,double eps=1E-4,int size=1) { 00217 KDL_CTX; 00218 Jacobian<A> a(size); 00219 random(a); 00220 Jacobian<B> b(size); 00221 random(b); 00222 KDL_ARG2(a,b); 00223 int i; 00224 for (i=0;i<a.nrOfDeriv();++i) { 00225 //A Avalue = A(a.value(),a.deriv(i).value()); 00226 //B Bvalue = B(b.value(),b.deriv(i).value()); 00227 KDL_MSG("testing value() component of derivVV "); 00228 checkEqual( 00229 myOp::derivVV(a.value(),a.deriv(i),b.value(),b.deriv(i)).value(), 00230 diff( 00231 myOp::value(a.value(),b.value()), 00232 myOp::value( 00233 addDelta(a.value(),a.deriv(i).value(),dt), 00234 addDelta(b.value(),b.deriv(i).value(),dt) 00235 ), 00236 dt), 00237 eps 00238 ); 00239 typename Traits<A>::derivType da1( 00240 addDelta(a.deriv(i).value(), a.deriv(i).deriv(),dt)); 00241 typename Traits<A>::valueType a1( 00242 addDelta(a.value(),a.deriv(i).value(),dt) 00243 ); 00244 typename Traits<B>::derivType db1( 00245 addDelta(b.deriv(i).value(), b.deriv(i).deriv(),dt)); 00246 typename Traits<B>::valueType b1( 00247 addDelta(b.value(),b.deriv(i).value(),dt) 00248 ); 00249 00250 KDL_MSG("testing deriv() components of derivVV "); 00251 checkEqual( 00252 myOp::derivVV(a.value(),a.deriv(i),b.value(),b.deriv(i)).deriv(), 00253 diff( 00254 myOp::derivVV(a.value(),a.deriv(i),b.value(),b.deriv(i)).value(), 00255 myOp::derivVV(a1, da1,b1,db1).value(), 00256 dt), 00257 eps 00258 ); 00259 KDL_MSG("testing deriv() components of derivVC "); 00260 checkEqual( 00261 myOp::derivVC(a.value(),a.deriv(i),b.value()).deriv(), 00262 diff( 00263 myOp::derivVC(a.value(),a.deriv(i),b.value()).value(), 00264 myOp::derivVC(a1, da1,b.value()).value(), 00265 dt), 00266 eps 00267 ); 00268 KDL_MSG("testing deriv() components of derivCV "); 00269 checkEqual( 00270 myOp::derivCV(a.value(),b.value(),b.deriv(i)).deriv(), 00271 diff( 00272 myOp::derivCV(a.value(),b.value(),b.deriv(i)).value(), 00273 myOp::derivCV(a.value(),b1,db1).value(), 00274 dt), 00275 eps 00276 ); 00277 00278 KDL_MSG("testing value() components of derivVC "); 00279 checkEqual( 00280 myOp::derivVC(a.value(),a.deriv(i),b.value()).value(), 00281 diff( 00282 myOp::value(a.value(),b.value()), 00283 myOp::value( 00284 addDelta(a.value(),a.deriv(i).value(),dt), 00285 b.value() 00286 ), 00287 dt), 00288 eps 00289 ); 00290 KDL_MSG("testing value() components of derivCV "); 00291 checkEqual( 00292 myOp::derivCV(a.value(),b.value(),b.deriv(i)).value(), 00293 diff( 00294 myOp::value(a.value(),b.value()), 00295 myOp::value( 00296 a.value(), 00297 addDelta(b.value(),b.deriv(i).value(),dt) 00298 ), 00299 dt), 00300 eps 00301 ); 00302 } 00303 } 00304 }; 00305 00306 00307 } // namespace 00308 #endif 00309 00310 00311 00312