orocos_kdl
rall1d.h
Go to the documentation of this file.
1 
2 /*****************************************************************************
3  * \file
4  * class for automatic differentiation on scalar values and 1st
5  * derivatives .
6  *
7  * \author
8  * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
9  *
10  * \version
11  * ORO_Geometry V0.2
12  *
13  * \par Note
14  * VC6++ contains a bug, concerning the use of inlined friend functions
15  * in combination with namespaces. So, try to avoid inlined friend
16  * functions !
17  *
18  * \par History
19  * - $log$
20  *
21  * \par Release
22  * $Id: rall1d.h,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $
23  * $Name: $
24  ****************************************************************************/
25 
26 #ifndef Rall1D_H
27 #define Rall1D_H
28 #include <assert.h>
29 #include "utility.h"
30 
31 namespace KDL {
48 template <typename T,typename V=T,typename S=T>
49 class Rall1d
50  {
51  public:
52  typedef T valuetype;
53  typedef V gradienttype;
54  typedef S scalartype;
55  public :
56  T t;
57  V grad;
58  public :
59  INLINE Rall1d():t(),grad() {};
60 
61  T value() const {
62  return t;
63  }
64  V deriv() const {
65  return grad;
66  }
67 
68  explicit INLINE Rall1d(typename TI<T>::Arg c)
69  {t=T(c);SetToZero(grad);}
70 
71  INLINE Rall1d(typename TI<T>::Arg tn, typename TI<V>::Arg afg):t(tn),grad(afg) {}
72 
73  INLINE Rall1d(const Rall1d<T,V,S>& r):t(r.t),grad(r.grad) {}
74  //if one defines this constructor, it's better optimized then the
75  //automatically generated one ( this one set's up a loop to copy
76  // word by word.
77 
78  INLINE T& Value() {
79  return t;
80  }
81 
82  INLINE V& Gradient() {
83  return grad;
84  }
85 
86  INLINE static Rall1d<T,V,S> Zero() {
87  Rall1d<T,V,S> tmp;
88  SetToZero(tmp);
89  return tmp;
90  }
91  INLINE static Rall1d<T,V,S> Identity() {
92  Rall1d<T,V,S> tmp;
93  SetToIdentity(tmp);
94  return tmp;
95  }
96 
98  {t=c;SetToZero(grad);return *this;}
99 
101  {t=r.t;grad=r.grad;return *this;}
102 
104  {
105  grad = LinComb(rhs.t,grad,-t,rhs.grad) / (rhs.t*rhs.t);
106  t /= rhs.t;
107  return *this;
108  }
109 
111  {
112  LinCombR(rhs.t,grad,t,rhs.grad,grad);
113  t *= rhs.t;
114  return *this;
115  }
116 
118  {
119  grad +=rhs.grad;
120  t +=rhs.t;
121  return *this;
122  }
123 
125  {
126  grad -= rhs.grad;
127  t -= rhs.t;
128  return *this;
129  }
130 
132  {
133  grad /= rhs;
134  t /= rhs;
135  return *this;
136  }
137 
139  {
140  grad *= rhs;
141  t *= rhs;
142  return *this;
143  }
144 
146  {
147  t += rhs;
148  return *this;
149  }
150 
152  {
153  t -= rhs;
154  return *this;
155  }
156 
157 
158 
159  // = operators
160  /* gives warnings on cygwin
161 
162  template <class T2,class V2,class S2>
163  friend INLINE Rall1d<T2,V2,S2> operator /(const Rall1d<T2,V2,S2>& lhs,const Rall1d<T2,V2,S2>& rhs);
164 
165  friend INLINE Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs);
166  friend INLINE Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs);
167  friend INLINE Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs);
168  friend INLINE Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& arg);
169  friend INLINE Rall1d<T,V,S> operator *(S s,const Rall1d<T,V,S>& v);
170  friend INLINE Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& v,S s);
171  friend INLINE Rall1d<T,V,S> operator +(S s,const Rall1d<T,V,S>& v);
172  friend INLINE Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& v,S s);
173  friend INLINE Rall1d<T,V,S> operator -(S s,const Rall1d<T,V,S>& v);
174  friend INLINE Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& v,S s);
175  friend INLINE Rall1d<T,V,S> operator /(S s,const Rall1d<T,V,S>& v);
176  friend INLINE Rall1d<T,V,S> operator /(const Rall1d<T,V,S>& v,S s);
177 
178  // = Mathematical functions that operate on Rall1d objects
179  friend INLINE Rall1d<T,V,S> exp(const Rall1d<T,V,S>& arg);
180  friend INLINE Rall1d<T,V,S> log(const Rall1d<T,V,S>& arg);
181  friend INLINE Rall1d<T,V,S> sin(const Rall1d<T,V,S>& arg);
182  friend INLINE Rall1d<T,V,S> cos(const Rall1d<T,V,S>& arg);
183  friend INLINE Rall1d<T,V,S> tan(const Rall1d<T,V,S>& arg);
184  friend INLINE Rall1d<T,V,S> sinh(const Rall1d<T,V,S>& arg);
185  friend INLINE Rall1d<T,V,S> cosh(const Rall1d<T,V,S>& arg);
186  friend INLINE Rall1d<T,V,S> sqr(const Rall1d<T,V,S>& arg);
187  friend INLINE Rall1d<T,V,S> pow(const Rall1d<T,V,S>& arg,double m) ;
188  friend INLINE Rall1d<T,V,S> sqrt(const Rall1d<T,V,S>& arg);
189  friend INLINE Rall1d<T,V,S> atan(const Rall1d<T,V,S>& x);
190  friend INLINE Rall1d<T,V,S> hypot(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x);
191  friend INLINE Rall1d<T,V,S> asin(const Rall1d<T,V,S>& x);
192  friend INLINE Rall1d<T,V,S> acos(const Rall1d<T,V,S>& x);
193  friend INLINE Rall1d<T,V,S> abs(const Rall1d<T,V,S>& x);
194  friend INLINE S Norm(const Rall1d<T,V,S>& value) ;
195  friend INLINE Rall1d<T,V,S> tanh(const Rall1d<T,V,S>& arg);
196  friend INLINE Rall1d<T,V,S> atan2(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x);
197 
198  // = Utility functions to improve performance
199 
200  friend INLINE Rall1d<T,V,S> LinComb(S alfa,const Rall1d<T,V,S>& a,
201  const T& beta,const Rall1d<T,V,S>& b );
202 
203  friend INLINE void LinCombR(S alfa,const Rall1d<T,V,S>& a,
204  const T& beta,const Rall1d<T,V,S>& b,Rall1d<T,V,S>& result );
205 
206  // = Setting value of a Rall1d object to 0 or 1
207 
208  friend INLINE void SetToZero(Rall1d<T,V,S>& value);
209  friend INLINE void SetToOne(Rall1d<T,V,S>& value);
210  // = Equality in an eps-interval
211  friend INLINE bool Equal(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x,double eps);
212  */
213  };
214 
215 
216 template <class T,class V,class S>
218  {
219  return Rall1d<T,V,S>(lhs.t/rhs.t,(lhs.grad*rhs.t-lhs.t*rhs.grad)/(rhs.t*rhs.t));
220  }
221 
222 template <class T,class V,class S>
223 INLINE Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs)
224  {
225  return Rall1d<T,V,S>(lhs.t*rhs.t,rhs.t*lhs.grad+lhs.t*rhs.grad);
226  }
227 
228 template <class T,class V,class S>
229 INLINE Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs)
230  {
231  return Rall1d<T,V,S>(lhs.t+rhs.t,lhs.grad+rhs.grad);
232  }
233 
234 
235 template <class T,class V,class S>
236 INLINE Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs)
237  {
238  return Rall1d<T,V,S>(lhs.t-rhs.t,lhs.grad-rhs.grad);
239  }
240 
241 template <class T,class V,class S>
242 INLINE Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& arg)
243  {
244  return Rall1d<T,V,S>(-arg.t,-arg.grad);
245  }
246 
247 template <class T,class V,class S>
248 INLINE Rall1d<T,V,S> operator *(S s,const Rall1d<T,V,S>& v)
249  {
250  return Rall1d<T,V,S>(s*v.t,s*v.grad);
251  }
252 
253 template <class T,class V,class S>
254 INLINE Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& v,S s)
255  {
256  return Rall1d<T,V,S>(v.t*s,v.grad*s);
257  }
258 
259 template <class T,class V,class S>
261  {
262  return Rall1d<T,V,S>(s+v.t,v.grad);
263  }
264 
265 template <class T,class V,class S>
267  {
268  return Rall1d<T,V,S>(v.t+s,v.grad);
269  }
270 
271 template <class T,class V,class S>
273  {
274  return Rall1d<T,V,S>(s-v.t,-v.grad);
275  }
276 
277 template <class T,class V,class S>
279  {
280  return Rall1d<T,V,S>(v.t-s,v.grad);
281  }
282 
283 template <class T,class V,class S>
285  {
286  return Rall1d<T,V,S>(s/v.t,(-s*v.grad)/(v.t*v.t));
287  }
288 
289 template <class T,class V,class S>
291  {
292  return Rall1d<T,V,S>(v.t/s,v.grad/s);
293  }
294 
295 
296 template <class T,class V,class S>
298  {
299  T v;
300  v= (exp(arg.t));
301  return Rall1d<T,V,S>(v,v*arg.grad);
302  }
303 
304 template <class T,class V,class S>
305 INLINE Rall1d<T,V,S> log(const Rall1d<T,V,S>& arg)
306  {
307  T v;
308  v=(log(arg.t));
309  return Rall1d<T,V,S>(v,arg.grad/arg.t);
310  }
311 
312 template <class T,class V,class S>
314  {
315  T v;
316  v=(sin(arg.t));
317  return Rall1d<T,V,S>(v,cos(arg.t)*arg.grad);
318  }
319 
320 template <class T,class V,class S>
322  {
323  T v;
324  v=(cos(arg.t));
325  return Rall1d<T,V,S>(v,-sin(arg.t)*arg.grad);
326  }
327 
328 template <class T,class V,class S>
330  {
331  T v;
332  v=(tan(arg.t));
333  return Rall1d<T,V,S>(v,arg.grad/sqr(cos(arg.t)));
334  }
335 
336 template <class T,class V,class S>
338  {
339  T v;
340  v=(sinh(arg.t));
341  return Rall1d<T,V,S>(v,cosh(arg.t)*arg.grad);
342  }
343 
344 template <class T,class V,class S>
346  {
347  T v;
348  v=(cosh(arg.t));
349  return Rall1d<T,V,S>(v,sinh(arg.t)*arg.grad);
350  }
351 
352 template <class T,class V,class S>
354  {
355  T v;
356  v=(arg.t*arg.t);
357  return Rall1d<T,V,S>(v,(2.0*arg.t)*arg.grad);
358  }
359 
360 template <class T,class V,class S>
361 INLINE Rall1d<T,V,S> pow(const Rall1d<T,V,S>& arg,double m)
362  {
363  T v;
364  v=(pow(arg.t,m));
365  return Rall1d<T,V,S>(v,(m*v/arg.t)*arg.grad);
366  }
367 
368 template <class T,class V,class S>
370  {
371  T v;
372  v=sqrt(arg.t);
373  return Rall1d<T,V,S>(v, (0.5/v)*arg.grad);
374  }
375 
376 template <class T,class V,class S>
378 {
379  T v;
380  v=(atan(x.t));
381  return Rall1d<T,V,S>(v,x.grad/(1.0+sqr(x.t)));
382 }
383 
384 template <class T,class V,class S>
386 {
387  T v;
388  v=(hypot(y.t,x.t));
389  return Rall1d<T,V,S>(v,(x.t/v)*x.grad+(y.t/v)*y.grad);
390 }
391 
392 template <class T,class V,class S>
394 {
395  T v;
396  v=(asin(x.t));
397  return Rall1d<T,V,S>(v,x.grad/sqrt(1.0-sqr(x.t)));
398 }
399 
400 template <class T,class V,class S>
402 {
403  T v;
404  v=(acos(x.t));
405  return Rall1d<T,V,S>(v,-x.grad/sqrt(1.0-sqr(x.t)));
406 }
407 
408 template <class T,class V,class S>
410 {
411  T v;
412  v=(Sign(x));
413  return Rall1d<T,V,S>(v*x,v*x.grad);
414 }
415 
416 
417 template <class T,class V,class S>
418 INLINE S Norm(const Rall1d<T,V,S>& value)
419 {
420  return Norm(value.t);
421 }
422 
423 template <class T,class V,class S>
425 {
426  T v(tanh(arg.t));
427  return Rall1d<T,V,S>(v,arg.grad/sqr(cosh(arg.t)));
428 }
429 
430 template <class T,class V,class S>
432 {
433  T v(x.t*x.t+y.t*y.t);
434  return Rall1d<T,V,S>(atan2(y.t,x.t),(x.t*y.grad-y.t*x.grad)/v);
435 }
436 
437 
438 template <class T,class V,class S>
439 INLINE Rall1d<T,V,S> LinComb(S alfa,const Rall1d<T,V,S>& a,
440  const T& beta,const Rall1d<T,V,S>& b ) {
441  return Rall1d<T,V,S>(
442  LinComb(alfa,a.t,beta,b.t),
443  LinComb(alfa,a.grad,beta,b.grad)
444  );
445 }
446 
447 template <class T,class V,class S>
448 INLINE void LinCombR(S alfa,const Rall1d<T,V,S>& a,
449  const T& beta,const Rall1d<T,V,S>& b,Rall1d<T,V,S>& result ) {
450  LinCombR(alfa, a.t, beta, b.t, result.t);
451  LinCombR(alfa, a.grad, beta, b.grad, result.grad);
452 }
453 
454 
455 template <class T,class V,class S>
456 INLINE void SetToZero(Rall1d<T,V,S>& value)
457  {
458  SetToZero(value.grad);
459  SetToZero(value.t);
460  }
461 template <class T,class V,class S>
463  {
464  SetToIdentity(value.t);
465  SetToZero(value.grad);
466  }
467 
468 template <class T,class V,class S>
469 INLINE bool Equal(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x,double eps=epsilon)
470 {
471  return (Equal(x.t,y.t,eps)&&Equal(x.grad,y.grad,eps));
472 }
473 
474 template <class T,class V,class S>
475 INLINE bool operator==(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x)
476 {
477 #ifdef KDL_USE_EQUAL
478  return Equal(y, x);
479 #else
480  return (x.t == y.t && x.grad == y.grad);
481 #endif
482 }
483 
484 template <class T,class V,class S>
485 INLINE bool operator!=(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x)
486 {
487  return !operator==(y, x);
488 }
489 
490 }
491 
492 
493 
494 #endif
KDL::SetToIdentity
INLINE void SetToIdentity(Rall1d< T, V, S > &value)
Definition: rall1d.h:484
KDL::Rall1d::operator/=
INLINE Rall1d< T, V, S > & operator/=(const Rall1d< T, V, S > &rhs)
Definition: rall1d.h:147
KDL::pow
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
Definition: rall1d.h:383
KDL::operator*
ArticulatedBodyInertia operator*(double a, const ArticulatedBodyInertia &I)
Definition: articulatedbodyinertia.cpp:49
KDL::Rall1d::valuetype
T valuetype
Definition: rall1d.h:96
KDL::Rall1d::scalartype
S scalartype
Definition: rall1d.h:98
KDL::sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:391
INLINE
#define INLINE
Definition: utility.h:227
KDL::asin
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
Definition: rall1d.h:415
KDL::Rall1d::deriv
V deriv() const
Definition: rall1d.h:108
utility.h
KDL::sin
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:335
KDL::Rall1d::grad
V grad
gradient
Definition: rall1d.h:101
KDL::operator/
INLINE Rall1d< T, V, S > operator/(const Rall1d< T, V, S > &lhs, const Rall1d< T, V, S > &rhs)
Definition: rall1d.h:239
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::operator==
bool operator==(const Rotation &a, const Rotation &b)
Definition: frames.cpp:455
KDL::cos
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:343
KDL::epsilon
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
Definition: utility.cxx:21
KDL::atan2
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
Definition: rall1d.h:453
KDL::Norm
INLINE S Norm(const Rall1d< T, V, S > &value)
Definition: rall1d.h:440
KDL::Rall1d::Value
INLINE T & Value()
Definition: rall1d.h:122
KDL::operator-
ArticulatedBodyInertia operator-(const ArticulatedBodyInertia &Ia, const ArticulatedBodyInertia &Ib)
Definition: articulatedbodyinertia.cpp:60
KDL::tan
INLINE Rall1d< T, V, S > tan(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:351
KDL::Rall1d::t
T t
value
Definition: rall1d.h:100
KDL::Rall1d::value
T value() const
Definition: rall1d.h:105
KDL::LinComb
INLINE Rall1d< T, V, S > LinComb(S alfa, const Rall1d< T, V, S > &a, const T &beta, const Rall1d< T, V, S > &b)
Definition: rall1d.h:461
KDL::Rall1d::Identity
static INLINE Rall1d< T, V, S > Identity()
Definition: rall1d.h:135
KDL::operator!=
INLINE bool operator!=(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
Definition: rall1d.h:507
KDL::Rall1d::gradienttype
V gradienttype
Definition: rall1d.h:97
KDL::Rall1d::operator+=
INLINE Rall1d< T, V, S > & operator+=(const Rall1d< T, V, S > &rhs)
Definition: rall1d.h:161
KDL::Rall1d::Rall1d
INLINE Rall1d()
Definition: rall1d.h:103
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::Equal
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
KDL::LinCombR
INLINE void LinCombR(S alfa, const Rall1d< T, V, S > &a, const T &beta, const Rall1d< T, V, S > &b, Rall1d< T, V, S > &result)
Definition: rall1d.h:470
KDL::sinh
INLINE Rall1d< T, V, S > sinh(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:359
KDL::TI::Arg
const typedef T & Arg
Arg is used for passing the element to a function.
Definition: utility.h:146
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::Rall1d::operator*=
INLINE Rall1d< T, V, S > & operator*=(const Rall1d< T, V, S > &rhs)
Definition: rall1d.h:154
KDL::Rall1d::Zero
static INLINE Rall1d< T, V, S > Zero()
Definition: rall1d.h:130
KDL::cosh
INLINE Rall1d< T, V, S > cosh(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:367
KDL::Rall1d::operator=
INLINE Rall1d< T, V, S > & operator=(S c)
Definition: rall1d.h:141
KDL::operator+
ArticulatedBodyInertia operator+(const ArticulatedBodyInertia &Ia, const ArticulatedBodyInertia &Ib)
Definition: articulatedbodyinertia.cpp:53
KDL::SetToZero
void SetToZero(Jacobian &jac)
Definition: jacobian.cpp:79
KDL::Rall1d::Gradient
INLINE V & Gradient()
Definition: rall1d.h:126
KDL::sqr
INLINE Rall1d< T, V, S > sqr(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:375
KDL::Rall1d::operator-=
INLINE Rall1d< T, V, S > & operator-=(const Rall1d< T, V, S > &rhs)
Definition: rall1d.h:168
KDL::tanh
INLINE Rall1d< T, V, S > tanh(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:446
KDL::TI
Definition: utility.h:143
KDL::acos
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
Definition: rall1d.h:423
KDL::abs
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
Definition: rall1d.h:431