orocos_kdl
rallnumbertest.cpp
Go to the documentation of this file.
1 
15 #include <kdl/rall1d.h>
16 #include <kdl/rall1d_io.h>
17 #include <kdl/rall2d.h>
18 #include <kdl/rall2d_io.h>
19 #include <kdl/fvector.h>
20 #include <kdl/fvector_io.h>
21 #include <kdl/fvector2.h>
22 #include <kdl/fvector2_io.h>
23 #include <kdl/test_macros.h>
24 
25 //#include <fstream>
26 
27 
28 using namespace KDL;
29 
30 // Again something a little bit more complicated : autodiff to 2nd derivative with N variables
31 template <class T,int N>
32 class Rall2dN :
33  public Rall1d<Rall1d<T, FVector<T,N> >, FVector2<Rall1d<T,FVector<T,N> >,N,T>,T>
34 {
35  public:
36  Rall2dN() {}
37  // dd is an array of an array of doubles
38  // dd[i][j] is the derivative towards ith and jth variable
39  Rall2dN(T val,T d[N],T dd[N][N]) {
40  this->t.t = val;
41  this->t.grad= FVector<T,N>(d);
42  for (int i=0;i<N;i++) {
43  this->grad[i].t = d[i];
44  this->grad[i].grad = FVector<T,N>(dd[i]);
45  }
46  }
47 };
48 
49 // Again something a little bit more complicated : the Nth derivative can be defined in a recursive way
50 template <int N>
51 class RallNd :
52  public Rall1d< RallNd<N-1>, RallNd<N-1>, double >
53 {
54 public:
55  RallNd() {}
56  RallNd(const Rall1d< RallNd<N-1>, RallNd<N-1>,double>& arg) :
57  Rall1d< RallNd<N-1>, RallNd<N-1>,double>(arg) {}
58  RallNd(double value,double d[]) {
59  this->t = RallNd<N-1>(value,d);
60  this->grad = RallNd<N-1>(d[0],&d[1]);
61  }
62 };
63 
64 template <>
65 class RallNd<1> : public Rall1d<double> {
66 public:
67  RallNd() {}
68  RallNd(const Rall1d<double>& arg) :
69  Rall1d<double,double,double>(arg) {}
70  RallNd(double value,double d[]) {
71  t = value;
72  grad = d[0];
73  }
74 };
75 
76 
77 
78 
79 template <class T>
80 void TstArithm(T& a) {
81  KDL_CTX;
82  KDL_DISP(a);
83  T b(a);
84  T c;
85  c = a;
86  KDL_DIFF(b,a);
87  KDL_DIFF(c,a);
88  KDL_DIFF( (a*a)*a, a*(a*a) );
89  KDL_DIFF( (-a)+a,T::Zero() );
90  KDL_DIFF( 2.0*a, a+a );
91  KDL_DIFF( a*2.0, a+a );
92  KDL_DIFF( a+2.0*a, 3.0*a );
93  KDL_DIFF( (a+2.0)*a, a*a +2.0*a );
94  KDL_DIFF( ((a+a)+a),(a+(a+a)) );
95  KDL_DIFF( asin(sin(a)), a );
96  KDL_DIFF( atan(tan(a)), a );
97  KDL_DIFF( acos(cos(a)), a );
98  KDL_DIFF( tan(a), sin(a)/cos(a) );
99  KDL_DIFF( exp(log(a)), a );
100  KDL_DIFF( exp(log(a)*2.0),sqr(a) );
101  KDL_DIFF( exp(log(a)*3.5),pow(a,3.5) );
102  KDL_DIFF( sqr(sqrt(a)), a );
103  KDL_DIFF( 2.0*sin(a)*cos(a), sin(2.0*a) );
104  KDL_DIFF( (a*a)/a, a );
105  KDL_DIFF( (a*a*a)/(a*a), a );
106  KDL_DIFF( sqr(sin(a))+sqr(cos(a)),T::Identity() );
107  KDL_DIFF( sqr(cosh(a))-sqr(sinh(a)),T::Identity() );
108  KDL_DIFF( pow(pow(a,3.0),1.0/3) , a );
109  KDL_DIFF( hypot(3.0*a,4.0*a), 5.0*a);
110  KDL_DIFF( atan2(5.0*sin(a),5.0*cos(a)) , a );
111  KDL_DIFF( tanh(a) , sinh(a)/cosh(a) );
112 }
113 
114 
115 int main() {
116  KDL_CTX;
117  //DisplContext::display=true;
118  Rall1d<double> a(0.12345,1);
119  TstArithm(a);
120  const double pb[4] = { 1.0, 2.0, 3.0, 4.0 };
121  Rall1d<double,FVector<double,4> > b(0.12345,FVector<double,4>(pb));
122  TstArithm(b);
123 
124 
125  Rall2d<double> d(0.12345,-1,0.9);
126  TstArithm(d);
127 
128 
130  TstArithm(f);
131 
133  TstArithm(g);
134 
135  // something more complicated without helper classes :
136  Rall1d<Rall1d<double, FVector<double,2> >, FVector2<Rall1d<double,FVector<double,2> >,2,double>,double> h(
137  Rall1d<double, FVector<double,2> >(
138  1.3,
139  FVector<double,2>(2,3)
140  ),
141  FVector2<Rall1d<double,FVector<double,2> >,2,double> (
142  Rall1d<double,FVector<double,2> >(
143  2,
144  FVector<double,2>(5,6)
145  ),
146  Rall1d<double,FVector<double,2> >(
147  3,
148  FVector<double,2>(6,9)
149  )
150  )
151  );
152  TstArithm(h);
153 
154  // with a helper-class and 3 variables
155  double pj[] = {2.0,3.0,4.0};
156  double ppj[][3] = {{5.0,6.0,7.0},{6.0,8.0,9.0},{7.0,9.0,10.0}};
157  Rall2dN<double,3> j(1.3,pj,ppj);
158  TstArithm(j);
159 
160  // to calculate the Nth derivative :
161  double pk[] = {2.0,3.0,4.0,5.0};
162  RallNd<4> k(1.3,pk);
163  TstArithm(k);
164 
165  return 0;
166 }
TstArithm
void TstArithm(T &a)
Definition: rallnumbertest.cpp:80
KDL::pow
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
Definition: rall1d.h:383
RallNd
Definition: rallNd.h:61
KDL::sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:391
RallNd< 1 >::RallNd
RallNd()
Definition: rallnumbertest.cpp:67
KDL::asin
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
Definition: rall1d.h:415
KDL::sin
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:335
RallNd< 1 >::RallNd
RallNd(double value, double d[])
Definition: rallnumbertest.cpp:70
KDL::exp
INLINE Rall1d< T, V, S > exp(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:319
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::cos
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:343
RallNd::RallNd
RallNd()
Definition: rallnumbertest.cpp:55
KDL::atan2
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
Definition: rall1d.h:453
Rall2dN::Rall2dN
Rall2dN()
Definition: rallnumbertest.cpp:36
KDL::tan
INLINE Rall1d< T, V, S > tan(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:351
KDL::Rall2d
Definition: rall2d.h:76
Rall2dN
Definition: rallnumbertest.cpp:32
RallNd< 1 >::RallNd
RallNd(const Rall1d< double > &arg)
Definition: rallnumbertest.cpp:68
Rall2dN::Rall2dN
Rall2dN(T val, T d[N], T dd[N][N])
Definition: rallnumbertest.cpp:39
KDL::log
INLINE Rall1d< T, V, S > log(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:327
KDL::Rall1d
Definition: rall1d.h:71
KDL::sinh
INLINE Rall1d< T, V, S > sinh(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:359
RallNd::RallNd
RallNd(const Rall1d< RallNd< N-1 >, RallNd< N-1 >, double > &arg)
Definition: rallnumbertest.cpp:56
KDL::hypot
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
Definition: rall1d.h:407
KDL::atan
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
Definition: rall1d.h:399
KDL::cosh
INLINE Rall1d< T, V, S > cosh(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:367
RallNd::RallNd
RallNd(double value, double d[])
Definition: rallnumbertest.cpp:58
KDL::sqr
INLINE Rall1d< T, V, S > sqr(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:375
main
int main()
Definition: rallnumbertest.cpp:115
KDL::tanh
INLINE Rall1d< T, V, S > tanh(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:446
KDL::acos
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
Definition: rall1d.h:423