rallnumbertest.cpp
Go to the documentation of this file.
00001 
00015 #include <kdl/rall1d.h>
00016 #include <kdl/rall1d_io.h>
00017 #include <kdl/rall2d.h>
00018 #include <kdl/rall2d_io.h>
00019 #include <kdl/fvector.h>
00020 #include <kdl/fvector_io.h>
00021 #include <kdl/fvector2.h>
00022 #include <kdl/fvector2_io.h>
00023 #include <kdl/test_macros.h>
00024 
00025 //#include <fstream>
00026 
00027 
00028 using namespace KDL;
00029 using namespace std;
00030 
00031 // Again something a little bit more complicated : autodiff to 2nd derivative with N variables
00032 template <class T,int N>
00033 class Rall2dN : 
00034         public Rall1d<Rall1d<T, FVector<T,N> >, FVector2<Rall1d<T,FVector<T,N> >,N,T>,T>
00035 {
00036         public:
00037                 Rall2dN() {}
00038                 // dd is an array of an array of doubles
00039                 // dd[i][j] is the derivative towards ith and jth variable
00040                 Rall2dN(T val,T d[N],T dd[N][N]) {
00041                         this->t.t = val;
00042                         this->t.grad= FVector<T,N>(d);
00043                         for (int i=0;i<N;i++) {
00044                                 this->grad[i].t = d[i];
00045                                 this->grad[i].grad = FVector<T,N>(dd[i]);
00046                         }
00047                 }
00048 };
00049 
00050 // Again something a little bit more complicated : the Nth derivative can be defined in a recursive way
00051 template <int N>
00052 class RallNd :
00053         public Rall1d< RallNd<N-1>, RallNd<N-1>, double >
00054 {
00055 public:
00056         RallNd() {}
00057         RallNd(const Rall1d< RallNd<N-1>, RallNd<N-1>,double>& arg) : 
00058                 Rall1d< RallNd<N-1>, RallNd<N-1>,double>(arg) {}
00059         RallNd(double value,double d[]) {
00060                 this->t    = RallNd<N-1>(value,d);
00061                 this->grad = RallNd<N-1>(d[0],&d[1]);
00062         }
00063 };
00064 
00065 template <>
00066 class RallNd<1> : public Rall1d<double>  {
00067 public:
00068         RallNd() {}
00069         RallNd(const Rall1d<double>& arg) :
00070                 Rall1d<double,double,double>(arg) {}
00071         RallNd(double value,double d[]) {
00072                 t    = value;
00073                 grad = d[0];
00074         }
00075 };
00076 
00077         
00078 
00079 
00080 template <class T>
00081 void TstArithm(T& a) {
00082     KDL_CTX;
00083         KDL_DISP(a);
00084         T b(a);
00085         T c;
00086         c = a;
00087         KDL_DIFF(b,a);
00088         KDL_DIFF(c,a);
00089         KDL_DIFF( (a*a)*a, a*(a*a)          );
00090         KDL_DIFF( (-a)+a,T::Zero()                  );
00091         KDL_DIFF( 2.0*a, a+a );
00092         KDL_DIFF( a*2.0, a+a );
00093         KDL_DIFF( a+2.0*a, 3.0*a );
00094         KDL_DIFF( (a+2.0)*a, a*a +2.0*a );
00095         KDL_DIFF( ((a+a)+a),(a+(a+a))      );
00096         KDL_DIFF( asin(sin(a)),  a         );
00097         KDL_DIFF( atan(tan(a)),  a         );
00098         KDL_DIFF( acos(cos(a)),  a         );
00099         KDL_DIFF( tan(a),   sin(a)/cos(a)  );
00100         KDL_DIFF( exp(log(a)),  a          );
00101         KDL_DIFF( exp(log(a)*2.0),sqr(a)     );
00102         KDL_DIFF( exp(log(a)*3.5),pow(a,3.5) );
00103         KDL_DIFF( sqr(sqrt(a)), a         );
00104         KDL_DIFF( 2.0*sin(a)*cos(a), sin(2.0*a) );
00105         KDL_DIFF( (a*a)/a, a              );
00106         KDL_DIFF( (a*a*a)/(a*a), a              );
00107         KDL_DIFF( sqr(sin(a))+sqr(cos(a)),T::Identity() );
00108         KDL_DIFF( sqr(cosh(a))-sqr(sinh(a)),T::Identity() );
00109         KDL_DIFF( pow(pow(a,3.0),1.0/3) , a );
00110         KDL_DIFF( hypot(3.0*a,4.0*a),      5.0*a);
00111         KDL_DIFF( atan2(5.0*sin(a),5.0*cos(a)) , a );
00112         KDL_DIFF( tanh(a) , sinh(a)/cosh(a) );
00113 }
00114 
00115 
00116 int main() {
00117     KDL_CTX;
00118     //DisplContext::display=true;
00119         Rall1d<double> a(0.12345,1);
00120         TstArithm(a);
00121         const double pb[4] = { 1.0, 2.0, 3.0, 4.0 };
00122         Rall1d<double,FVector<double,4> > b(0.12345,FVector<double,4>(pb));
00123         TstArithm(b);
00124 
00125         
00126         Rall2d<double> d(0.12345,-1,0.9);
00127         TstArithm(d);
00128         
00129 
00130         Rall1d<Rall1d<double>,Rall1d<double>,double> f(Rall1d<double>(1,2),Rall1d<double>(2,3));
00131         TstArithm(f);
00132 
00133         Rall2d<Rall1d<double>,Rall1d<double>,double> g(Rall1d<double>(1,2),Rall1d<double>(2,3),Rall1d<double>(3,4));
00134         TstArithm(g);
00135         
00136         // something more complicated without helper classes :
00137         Rall1d<Rall1d<double, FVector<double,2> >, FVector2<Rall1d<double,FVector<double,2> >,2,double>,double> h(
00138                 Rall1d<double, FVector<double,2> >(
00139                         1.3,
00140                         FVector<double,2>(2,3)
00141                 ),
00142                 FVector2<Rall1d<double,FVector<double,2> >,2,double> (
00143                         Rall1d<double,FVector<double,2> >(
00144                                 2,
00145                                 FVector<double,2>(5,6)
00146                         ),
00147                         Rall1d<double,FVector<double,2> >(
00148                                 3,
00149                                 FVector<double,2>(6,9)
00150                         )
00151                 )
00152         );
00153         TstArithm(h);
00154         
00155         // with a helper-class and 3 variables
00156         double pj[] = {2.0,3.0,4.0};
00157         double ppj[][3] = {{5.0,6.0,7.0},{6.0,8.0,9.0},{7.0,9.0,10.0}};
00158         Rall2dN<double,3> j(1.3,pj,ppj);
00159         TstArithm(j);
00160 
00161         // to calculate the Nth derivative :
00162         double pk[] = {2.0,3.0,4.0,5.0};
00163         RallNd<4> k(1.3,pk);
00164         TstArithm(k);
00165 
00166         return 0;
00167 }


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