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
00226
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 }
00308 #endif
00309
00310
00311
00312