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
00026
00027
00028 using namespace KDL;
00029 using namespace std;
00030
00031
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
00039
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
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
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
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
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
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 }