jacobiantests.hpp
Go to the documentation of this file.
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 


orocos_kdl
Author(s):
autogenerated on Mon Oct 6 2014 03:11:16