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 using namespace std;
30 
31 // Again something a little bit more complicated : autodiff to 2nd derivative with N variables
32 template <class T,int N>
33 class Rall2dN :
34  public Rall1d<Rall1d<T, FVector<T,N> >, FVector2<Rall1d<T,FVector<T,N> >,N,T>,T>
35 {
36  public:
37  Rall2dN() {}
38  // dd is an array of an array of doubles
39  // dd[i][j] is the derivative towards ith and jth variable
40  Rall2dN(T val,T d[N],T dd[N][N]) {
41  this->t.t = val;
42  this->t.grad= FVector<T,N>(d);
43  for (int i=0;i<N;i++) {
44  this->grad[i].t = d[i];
45  this->grad[i].grad = FVector<T,N>(dd[i]);
46  }
47  }
48 };
49 
50 // Again something a little bit more complicated : the Nth derivative can be defined in a recursive way
51 template <int N>
52 class RallNd :
53  public Rall1d< RallNd<N-1>, RallNd<N-1>, double >
54 {
55 public:
56  RallNd() {}
57  RallNd(const Rall1d< RallNd<N-1>, RallNd<N-1>,double>& arg) :
58  Rall1d< RallNd<N-1>, RallNd<N-1>,double>(arg) {}
59  RallNd(double value,double d[]) {
60  this->t = RallNd<N-1>(value,d);
61  this->grad = RallNd<N-1>(d[0],&d[1]);
62  }
63 };
64 
65 template <>
66 class RallNd<1> : public Rall1d<double> {
67 public:
68  RallNd() {}
69  RallNd(const Rall1d<double>& arg) :
70  Rall1d<double,double,double>(arg) {}
71  RallNd(double value,double d[]) {
72  t = value;
73  grad = d[0];
74  }
75 };
76 
77 
78 
79 
80 template <class T>
81 void TstArithm(T& a) {
82  KDL_CTX;
83  KDL_DISP(a);
84  T b(a);
85  T c;
86  c = a;
87  KDL_DIFF(b,a);
88  KDL_DIFF(c,a);
89  KDL_DIFF( (a*a)*a, a*(a*a) );
90  KDL_DIFF( (-a)+a,T::Zero() );
91  KDL_DIFF( 2.0*a, a+a );
92  KDL_DIFF( a*2.0, a+a );
93  KDL_DIFF( a+2.0*a, 3.0*a );
94  KDL_DIFF( (a+2.0)*a, a*a +2.0*a );
95  KDL_DIFF( ((a+a)+a),(a+(a+a)) );
96  KDL_DIFF( asin(sin(a)), a );
97  KDL_DIFF( atan(tan(a)), a );
98  KDL_DIFF( acos(cos(a)), a );
99  KDL_DIFF( tan(a), sin(a)/cos(a) );
100  KDL_DIFF( exp(log(a)), a );
101  KDL_DIFF( exp(log(a)*2.0),sqr(a) );
102  KDL_DIFF( exp(log(a)*3.5),pow(a,3.5) );
103  KDL_DIFF( sqr(sqrt(a)), a );
104  KDL_DIFF( 2.0*sin(a)*cos(a), sin(2.0*a) );
105  KDL_DIFF( (a*a)/a, a );
106  KDL_DIFF( (a*a*a)/(a*a), a );
107  KDL_DIFF( sqr(sin(a))+sqr(cos(a)),T::Identity() );
108  KDL_DIFF( sqr(cosh(a))-sqr(sinh(a)),T::Identity() );
109  KDL_DIFF( pow(pow(a,3.0),1.0/3) , a );
110  KDL_DIFF( hypot(3.0*a,4.0*a), 5.0*a);
111  KDL_DIFF( atan2(5.0*sin(a),5.0*cos(a)) , a );
112  KDL_DIFF( tanh(a) , sinh(a)/cosh(a) );
113 }
114 
115 
116 int main() {
117  KDL_CTX;
118  //DisplContext::display=true;
119  Rall1d<double> a(0.12345,1);
120  TstArithm(a);
121  const double pb[4] = { 1.0, 2.0, 3.0, 4.0 };
122  Rall1d<double,FVector<double,4> > b(0.12345,FVector<double,4>(pb));
123  TstArithm(b);
124 
125 
126  Rall2d<double> d(0.12345,-1,0.9);
127  TstArithm(d);
128 
129 
130  Rall1d<Rall1d<double>,Rall1d<double>,double> f(Rall1d<double>(1,2),Rall1d<double>(2,3));
131  TstArithm(f);
132 
133  Rall2d<Rall1d<double>,Rall1d<double>,double> g(Rall1d<double>(1,2),Rall1d<double>(2,3),Rall1d<double>(3,4));
134  TstArithm(g);
135 
136  // something more complicated without helper classes :
137  Rall1d<Rall1d<double, FVector<double,2> >, FVector2<Rall1d<double,FVector<double,2> >,2,double>,double> h(
138  Rall1d<double, FVector<double,2> >(
139  1.3,
140  FVector<double,2>(2,3)
141  ),
142  FVector2<Rall1d<double,FVector<double,2> >,2,double> (
143  Rall1d<double,FVector<double,2> >(
144  2,
145  FVector<double,2>(5,6)
146  ),
147  Rall1d<double,FVector<double,2> >(
148  3,
149  FVector<double,2>(6,9)
150  )
151  )
152  );
153  TstArithm(h);
154 
155  // with a helper-class and 3 variables
156  double pj[] = {2.0,3.0,4.0};
157  double ppj[][3] = {{5.0,6.0,7.0},{6.0,8.0,9.0},{7.0,9.0,10.0}};
158  Rall2dN<double,3> j(1.3,pj,ppj);
159  TstArithm(j);
160 
161  // to calculate the Nth derivative :
162  double pk[] = {2.0,3.0,4.0,5.0};
163  RallNd<4> k(1.3,pk);
164  TstArithm(k);
165 
166  return 0;
167 }
Definition: rallNd.h:61
INLINE Rall1d< T, V, S > log(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:305
INLINE Rall1d< T, V, S > cosh(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:345
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
Definition: rall1d.h:385
INLINE Rall1d< T, V, S > sqr(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:353
INLINE Rall1d< T, V, S > sinh(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:337
Rall2dN(T val, T d[N], T dd[N][N])
RallNd(const Rall1d< double > &arg)
INLINE Rall1d< T, V, S > tanh(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:424
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
Definition: rall1d.h:377
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
Definition: rall1d.h:393
RallNd(double value, double d[])
int main()
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:369
INLINE Rall1d< T, V, S > exp(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:297
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
Definition: rall1d.h:361
RallNd(const Rall1d< RallNd< N-1 >, RallNd< N-1 >, double > &arg)
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
Definition: rall1d.h:401
void TstArithm(T &a)
RallNd(double value, double d[])
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
Definition: rall1d.h:431
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:321
INLINE Rall1d< T, V, S > tan(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:329
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:313


orocos_kdl
Author(s):
autogenerated on Sat Jun 15 2019 19:07:36