orocos_kdl
framevel.hpp
Go to the documentation of this file.
1 /*****************************************************************************
2  * \file
3  * This file contains the definition of classes for a
4  * Rall Algebra of (subset of) the classes defined in frames,
5  * i.e. classes that contain a pair (value,derivative) and define operations on that pair
6  * this classes are useful for automatic differentiation ( <-> symbolic diff , <-> numeric diff)
7  * Defines VectorVel, RotationVel, FrameVel. Look at Frames.h for details on how to work
8  * with Frame objects.
9  * \author
10  * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
11  *
12  * \version
13  * ORO_Geometry V0.2
14  *
15  * \par History
16  * - $log$
17  *
18  * \par Release
19  * $Id: rframes.h,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $
20  * $Name: $
21  ****************************************************************************/
22 
23 #ifndef KDL_FRAMEVEL_H
24 #define KDL_FRAMEVEL_H
25 
26 #include "utilities/utility.h"
27 #include "utilities/rall1d.h"
28 #include "utilities/traits.h"
29 
30 #include "frames.hpp"
31 
32 
33 namespace KDL {
34 
35 typedef Rall1d<double> doubleVel;
36 
37 IMETHOD doubleVel diff(const doubleVel& a,const doubleVel& b,double dt=1.0) {
38  return doubleVel((b.t-a.t)/dt,(b.grad-a.grad)/dt);
39 }
40 
41 IMETHOD doubleVel addDelta(const doubleVel& a,const doubleVel&da,double dt=1.0) {
42  return doubleVel(a.t+da.t*dt,a.grad+da.grad*dt);
43 }
44 
45 IMETHOD void random(doubleVel& F) {
46  random(F.t);
47  random(F.grad);
48 }
49 IMETHOD void posrandom(doubleVel& F) {
50  posrandom(F.t);
51  posrandom(F.grad);
52 }
53 
54 }
55 
56 template <>
57 struct Traits<KDL::doubleVel> {
58  typedef double valueType;
60 };
61 
62 namespace KDL {
63 
64 class TwistVel;
65 class VectorVel;
66 class FrameVel;
67 class RotationVel;
68 
69 // Equal is friend function, but default arguments for friends are forbidden (ยง8.3.6.4)
70 IMETHOD bool Equal(const VectorVel& r1,const VectorVel& r2,double eps=epsilon);
71 IMETHOD bool Equal(const Vector& r1,const VectorVel& r2,double eps=epsilon);
72 IMETHOD bool Equal(const VectorVel& r1,const Vector& r2,double eps=epsilon);
73 IMETHOD bool Equal(const RotationVel& r1,const RotationVel& r2,double eps=epsilon);
74 IMETHOD bool Equal(const Rotation& r1,const RotationVel& r2,double eps=epsilon);
75 IMETHOD bool Equal(const RotationVel& r1,const Rotation& r2,double eps=epsilon);
76 IMETHOD bool Equal(const FrameVel& r1,const FrameVel& r2,double eps=epsilon);
77 IMETHOD bool Equal(const Frame& r1,const FrameVel& r2,double eps=epsilon);
78 IMETHOD bool Equal(const FrameVel& r1,const Frame& r2,double eps=epsilon);
79 IMETHOD bool Equal(const TwistVel& a,const TwistVel& b,double eps=epsilon);
80 IMETHOD bool Equal(const Twist& a,const TwistVel& b,double eps=epsilon);
81 IMETHOD bool Equal(const TwistVel& a,const Twist& b,double eps=epsilon);
82 
83 
84 class VectorVel
85 // = TITLE
86 // An VectorVel is a Vector and its first derivative
87 // = CLASS TYPE
88 // Concrete
89 {
90 public:
91  Vector p; // position vector
92  Vector v; // velocity vector
93 public:
94  VectorVel():p(),v(){}
95  VectorVel(const Vector& _p,const Vector& _v):p(_p),v(_v) {}
96  explicit VectorVel(const Vector& _p):p(_p),v(Vector::Zero()) {}
97 
98  Vector value() const { return p;}
99  Vector deriv() const { return v;}
100 
101  IMETHOD VectorVel& operator = (const VectorVel& arg);
102  IMETHOD VectorVel& operator = (const Vector& arg);
105  IMETHOD static VectorVel Zero();
106  IMETHOD void ReverseSign();
107  IMETHOD doubleVel Norm(double eps=epsilon) const;
108  IMETHOD friend VectorVel operator + (const VectorVel& r1,const VectorVel& r2);
109  IMETHOD friend VectorVel operator - (const VectorVel& r1,const VectorVel& r2);
110  IMETHOD friend VectorVel operator + (const Vector& r1,const VectorVel& r2);
111  IMETHOD friend VectorVel operator - (const Vector& r1,const VectorVel& r2);
112  IMETHOD friend VectorVel operator + (const VectorVel& r1,const Vector& r2);
113  IMETHOD friend VectorVel operator - (const VectorVel& r1,const Vector& r2);
114  IMETHOD friend VectorVel operator * (const VectorVel& r1,const VectorVel& r2);
115  IMETHOD friend VectorVel operator * (const VectorVel& r1,const Vector& r2);
116  IMETHOD friend VectorVel operator * (const Vector& r1,const VectorVel& r2);
117  IMETHOD friend VectorVel operator * (const VectorVel& r1,double r2);
118  IMETHOD friend VectorVel operator * (double r1,const VectorVel& r2);
119  IMETHOD friend VectorVel operator * (const doubleVel& r1,const VectorVel& r2);
120  IMETHOD friend VectorVel operator * (const VectorVel& r2,const doubleVel& r1);
121  IMETHOD friend VectorVel operator*(const Rotation& R,const VectorVel& x);
122 
123  IMETHOD friend VectorVel operator / (const VectorVel& r1,double r2);
124  IMETHOD friend VectorVel operator / (const VectorVel& r2,const doubleVel& r1);
125  IMETHOD friend void SetToZero(VectorVel& v);
126 
127 
128  IMETHOD friend bool Equal(const VectorVel& r1,const VectorVel& r2,double eps);
129  IMETHOD friend bool Equal(const Vector& r1,const VectorVel& r2,double eps);
130  IMETHOD friend bool Equal(const VectorVel& r1,const Vector& r2,double eps);
131 
132  IMETHOD friend bool operator==(const VectorVel& r1,const VectorVel& r2);
133  IMETHOD friend bool operator!=(const VectorVel& r1,const VectorVel& r2);
134  IMETHOD friend bool operator==(const Vector& r1,const VectorVel& r2);
135  IMETHOD friend bool operator!=(const Vector& r1,const VectorVel& r2);
136  IMETHOD friend bool operator==(const VectorVel& r1,const Vector& r2);
137  IMETHOD friend bool operator!=(const VectorVel& r1,const Vector& r2);
138 
139  IMETHOD friend VectorVel operator - (const VectorVel& r);
140  IMETHOD friend doubleVel dot(const VectorVel& lhs,const VectorVel& rhs);
141  IMETHOD friend doubleVel dot(const VectorVel& lhs,const Vector& rhs);
142  IMETHOD friend doubleVel dot(const Vector& lhs,const VectorVel& rhs);
143 };
144 
145 
147 // = TITLE
148 // An RotationVel is a Rotation and its first derivative, a rotation vector
149 // = CLASS TYPE
150 // Concrete
151 {
152 public:
153  Rotation R; // Rotation matrix
154  Vector w; // rotation vector
155 public:
156  RotationVel():R(),w() {}
157  explicit RotationVel(const Rotation& _R):R(_R),w(Vector::Zero()){}
158  RotationVel(const Rotation& _R,const Vector& _w):R(_R),w(_w){}
159 
160 
161  Rotation value() const { return R;}
162  Vector deriv() const { return w;}
163 
164 
167  IMETHOD VectorVel UnitX() const;
168  IMETHOD VectorVel UnitY() const;
169  IMETHOD VectorVel UnitZ() const;
170  IMETHOD static RotationVel Identity();
171  IMETHOD RotationVel Inverse() const;
172  IMETHOD VectorVel Inverse(const VectorVel& arg) const;
173  IMETHOD VectorVel Inverse(const Vector& arg) const;
174  IMETHOD VectorVel operator*(const VectorVel& arg) const;
175  IMETHOD VectorVel operator*(const Vector& arg) const;
176  IMETHOD void DoRotX(const doubleVel& angle);
177  IMETHOD void DoRotY(const doubleVel& angle);
178  IMETHOD void DoRotZ(const doubleVel& angle);
179  IMETHOD static RotationVel RotX(const doubleVel& angle);
180  IMETHOD static RotationVel RotY(const doubleVel& angle);
181  IMETHOD static RotationVel RotZ(const doubleVel& angle);
182  IMETHOD static RotationVel Rot(const Vector& rotvec,const doubleVel& angle);
183  // rotvec has arbitrary norm
184  // rotation around a constant vector !
185  IMETHOD static RotationVel Rot2(const Vector& rotvec,const doubleVel& angle);
186  // rotvec is normalized.
187  // rotation around a constant vector !
188  IMETHOD friend RotationVel operator* (const RotationVel& r1,const RotationVel& r2);
189  IMETHOD friend RotationVel operator* (const Rotation& r1,const RotationVel& r2);
190  IMETHOD friend RotationVel operator* (const RotationVel& r1,const Rotation& r2);
191  IMETHOD friend bool Equal(const RotationVel& r1,const RotationVel& r2,double eps);
192  IMETHOD friend bool Equal(const Rotation& r1,const RotationVel& r2,double eps);
193  IMETHOD friend bool Equal(const RotationVel& r1,const Rotation& r2,double eps);
194 
195  IMETHOD friend bool operator==(const RotationVel& r1,const RotationVel& r2);
196  IMETHOD friend bool operator!=(const RotationVel& r1,const RotationVel& r2);
197  IMETHOD friend bool operator==(const Rotation& r1,const RotationVel& r2);
198  IMETHOD friend bool operator!=(const Rotation& r1,const RotationVel& r2);
199  IMETHOD friend bool operator==(const RotationVel& r1,const Rotation& r2);
200  IMETHOD friend bool operator!=(const RotationVel& r1,const Rotation& r2);
201 
202  IMETHOD TwistVel Inverse(const TwistVel& arg) const;
203  IMETHOD TwistVel Inverse(const Twist& arg) const;
204  IMETHOD TwistVel operator * (const TwistVel& arg) const;
205  IMETHOD TwistVel operator * (const Twist& arg) const;
206 };
207 
208 
209 class FrameVel
210 // = TITLE
211 // An FrameVel is a Frame and its first derivative, a Twist vector
212 // = CLASS TYPE
213 // Concrete
214 // = CAVEATS
215 //
216 {
217 public:
220 public:
222 
223  explicit FrameVel(const Frame& _T):
224  M(_T.M),p(_T.p) {}
225 
226  FrameVel(const Frame& _T,const Twist& _t):
227  M(_T.M,_t.rot),p(_T.p,_t.vel) {}
228 
229  FrameVel(const RotationVel& _M,const VectorVel& _p):
230  M(_M),p(_p) {}
231 
232 
233  Frame value() const { return Frame(M.value(),p.value());}
234  Twist deriv() const { return Twist(p.deriv(),M.deriv());}
235 
236 
237  IMETHOD FrameVel& operator = (const Frame& arg);
238  IMETHOD FrameVel& operator = (const FrameVel& arg);
239  IMETHOD static FrameVel Identity();
240  IMETHOD FrameVel Inverse() const;
241  IMETHOD VectorVel Inverse(const VectorVel& arg) const;
242  IMETHOD VectorVel operator*(const VectorVel& arg) const;
243  IMETHOD VectorVel operator*(const Vector& arg) const;
244  IMETHOD VectorVel Inverse(const Vector& arg) const;
245  IMETHOD Frame GetFrame() const;
246  IMETHOD Twist GetTwist() const;
247  IMETHOD friend FrameVel operator * (const FrameVel& f1,const FrameVel& f2);
248  IMETHOD friend FrameVel operator * (const Frame& f1,const FrameVel& f2);
249  IMETHOD friend FrameVel operator * (const FrameVel& f1,const Frame& f2);
250  IMETHOD friend bool Equal(const FrameVel& r1,const FrameVel& r2,double eps);
251  IMETHOD friend bool Equal(const Frame& r1,const FrameVel& r2,double eps);
252  IMETHOD friend bool Equal(const FrameVel& r1,const Frame& r2,double eps);
253 
254  IMETHOD friend bool operator==(const FrameVel& a,const FrameVel& b);
255  IMETHOD friend bool operator!=(const FrameVel& a,const FrameVel& b);
256  IMETHOD friend bool operator==(const Frame& a,const FrameVel& b);
257  IMETHOD friend bool operator!=(const Frame& a,const FrameVel& b);
258  IMETHOD friend bool operator==(const FrameVel& a,const Frame& b);
259  IMETHOD friend bool operator!=(const FrameVel& a,const Frame& b);
260 
261  IMETHOD TwistVel Inverse(const TwistVel& arg) const;
262  IMETHOD TwistVel Inverse(const Twist& arg) const;
263  IMETHOD TwistVel operator * (const TwistVel& arg) const;
264  IMETHOD TwistVel operator * (const Twist& arg) const;
265 };
266 
267 
268 //very similar to Wrench class.
269 class TwistVel
270 // = TITLE
271 // This class represents a TwistVel. This is a velocity and rotational velocity together
272 {
273 public:
276 public:
277 
278 // = Constructors
279  TwistVel():vel(),rot() {};
280  TwistVel(const VectorVel& _vel,const VectorVel& _rot):vel(_vel),rot(_rot) {};
281  TwistVel(const Twist& p,const Twist& v):vel(p.vel, v.vel), rot( p.rot, v.rot) {};
282  TwistVel(const Twist& p):vel(p.vel), rot( p.rot) {};
283 
284  Twist value() const {
285  return Twist(vel.value(),rot.value());
286  }
287  Twist deriv() const {
288  return Twist(vel.deriv(),rot.deriv());
289  }
290 // = Operators
291  IMETHOD TwistVel& operator-=(const TwistVel& arg);
292  IMETHOD TwistVel& operator+=(const TwistVel& arg);
293 
294 // = External operators
295  IMETHOD friend TwistVel operator*(const TwistVel& lhs,double rhs);
296  IMETHOD friend TwistVel operator*(double lhs,const TwistVel& rhs);
297  IMETHOD friend TwistVel operator/(const TwistVel& lhs,double rhs);
298 
299  IMETHOD friend TwistVel operator*(const TwistVel& lhs,const doubleVel& rhs);
300  IMETHOD friend TwistVel operator*(const doubleVel& lhs,const TwistVel& rhs);
301  IMETHOD friend TwistVel operator/(const TwistVel& lhs,const doubleVel& rhs);
302 
303  IMETHOD friend TwistVel operator+(const TwistVel& lhs,const TwistVel& rhs);
304  IMETHOD friend TwistVel operator-(const TwistVel& lhs,const TwistVel& rhs);
305  IMETHOD friend TwistVel operator-(const TwistVel& arg);
306  IMETHOD friend void SetToZero(TwistVel& v);
307 
308 
309 // = Zero
310  static IMETHOD TwistVel Zero();
311 
312 // = Reverse Sign
313  IMETHOD void ReverseSign();
314 
315 // = Change Reference point
316  IMETHOD TwistVel RefPoint(const VectorVel& v_base_AB);
317  // Changes the reference point of the TwistVel.
318  // The VectorVel v_base_AB is expressed in the same base as the TwistVel
319  // The VectorVel v_base_AB is a VectorVel from the old point to
320  // the new point.
321  // Complexity : 6M+6A
322 
323  // = Equality operators
324  // do not use operator == because the definition of Equal(.,.) is slightly
325  // different. It compares whether the 2 arguments are equal in an eps-interval
326  IMETHOD friend bool Equal(const TwistVel& a,const TwistVel& b,double eps);
327  IMETHOD friend bool Equal(const Twist& a,const TwistVel& b,double eps);
328  IMETHOD friend bool Equal(const TwistVel& a,const Twist& b,double eps);
329 
330  IMETHOD friend bool operator==(const TwistVel& a,const TwistVel& b);
331  IMETHOD friend bool operator!=(const TwistVel& a,const TwistVel& b);
332  IMETHOD friend bool operator==(const Twist& a,const TwistVel& b);
333  IMETHOD friend bool operator!=(const Twist& a,const TwistVel& b);
334  IMETHOD friend bool operator==(const TwistVel& a,const Twist& b);
335  IMETHOD friend bool operator!=(const TwistVel& a,const Twist& b);
336 
337 // = Conversion to other entities
338  IMETHOD Twist GetTwist() const;
339  IMETHOD Twist GetTwistDot() const;
340 // = Friends
341  friend class RotationVel;
342  friend class FrameVel;
343 
344 };
345 
346 IMETHOD VectorVel diff(const VectorVel& a,const VectorVel& b,double dt=1.0) {
347  return VectorVel(diff(a.p,b.p,dt),diff(a.v,b.v,dt));
348 }
349 
350 IMETHOD VectorVel addDelta(const VectorVel& a,const VectorVel&da,double dt=1.0) {
351  return VectorVel(addDelta(a.p,da.p,dt),addDelta(a.v,da.v,dt));
352 }
353 IMETHOD VectorVel diff(const RotationVel& a,const RotationVel& b,double dt = 1.0) {
354  return VectorVel(diff(a.R,b.R,dt),diff(a.w,b.w,dt));
355 }
356 
357 IMETHOD RotationVel addDelta(const RotationVel& a,const VectorVel&da,double dt=1.0) {
358  return RotationVel(addDelta(a.R,da.p,dt),addDelta(a.w,da.v,dt));
359 }
360 
361 IMETHOD TwistVel diff(const FrameVel& a,const FrameVel& b,double dt=1.0) {
362  return TwistVel(diff(a.M,b.M,dt),diff(a.p,b.p,dt));
363 }
364 
365 IMETHOD FrameVel addDelta(const FrameVel& a,const TwistVel& da,double dt=1.0) {
366  return FrameVel(
367  addDelta(a.M,da.rot,dt),
368  addDelta(a.p,da.vel,dt)
369  );
370 }
371 
373  random(a.p);
374  random(a.v);
375 }
377  random(a.vel);
378  random(a.rot);
379 }
380 
382  random(R.R);
383  random(R.w);
384 }
385 
387  random(F.M);
388  random(F.p);
389 }
391  posrandom(a.p);
392  posrandom(a.v);
393 }
395  posrandom(a.vel);
396  posrandom(a.rot);
397 }
398 
400  posrandom(R.R);
401  posrandom(R.w);
402 }
403 
405  posrandom(F.M);
406  posrandom(F.p);
407 }
408 
409 #ifdef KDL_INLINE
410 #include "framevel.inl"
411 #endif
412 
413 } // namespace KDL
414 
415 template<> struct std::hash<KDL::doubleVel>
416 {
417  std::size_t operator()(KDL::doubleVel const& dv) const noexcept
418  {
419  size_t seed = 0;
420  KDL::hash_combine(seed, dv.value());
421  KDL::hash_combine(seed, dv.deriv());
422  return seed;
423  }
424 };
425 
426 template<> struct std::hash<KDL::VectorVel>
427 {
428  std::size_t operator()(KDL::VectorVel const& vv) const noexcept
429  {
430  size_t seed = 0;
431  KDL::hash_combine(seed, vv.p);
432  KDL::hash_combine(seed, vv.v);
433  return seed;
434  }
435 };
436 
437 template<> struct std::hash<KDL::RotationVel>
438 {
439  std::size_t operator()(KDL::RotationVel const& rv) const noexcept
440  {
441  size_t seed = 0;
442  KDL::hash_combine(seed, rv.R);
443  KDL::hash_combine(seed, rv.w);
444  return seed;
445  }
446 };
447 
448 template<> struct std::hash<KDL::FrameVel>
449 {
450  std::size_t operator()(KDL::FrameVel const& fv) const noexcept
451  {
452  size_t seed = 0;
453  KDL::hash_combine(seed, fv.M);
454  KDL::hash_combine(seed, fv.p);
455  return seed;
456  }
457 };
458 
459 template<> struct std::hash<KDL::TwistVel>
460 {
461  std::size_t operator()(KDL::TwistVel const& tv) const noexcept
462  {
463  size_t seed = 0;
464  KDL::hash_combine(seed, tv.vel);
465  KDL::hash_combine(seed, tv.rot);
466  return seed;
467  }
468 };
469 
470 #endif
KDL::TwistVel::TwistVel
TwistVel(const VectorVel &_vel, const VectorVel &_rot)
Definition: framevel.hpp:280
KDL::VectorVel::operator-=
IMETHOD VectorVel & operator-=(const VectorVel &arg)
Definition: framevel.inl:340
KDL::RotationVel::RotY
static IMETHOD RotationVel RotY(const doubleVel &angle)
Definition: framevel.inl:216
KDL::TwistVel::operator/
IMETHOD friend TwistVel operator/(const TwistVel &lhs, double rhs)
KDL::RotationVel::RotationVel
RotationVel()
Definition: framevel.hpp:156
KDL::VectorVel::value
Vector value() const
Definition: framevel.hpp:98
frames.hpp
KDL::RotationVel::w
Vector w
Definition: framevel.hpp:154
std::hash< KDL::RotationVel >::operator()
std::size_t operator()(KDL::RotationVel const &rv) const noexcept
Definition: framevel.hpp:439
KDL::TwistVel::operator*
IMETHOD friend TwistVel operator*(const TwistVel &lhs, double rhs)
KDL::RotationVel::RotationVel
RotationVel(const Rotation &_R, const Vector &_w)
Definition: framevel.hpp:158
KDL::TwistVel::GetTwist
IMETHOD Twist GetTwist() const
Definition: framevel.inl:655
KDL::FrameVel::deriv
Twist deriv() const
Definition: framevel.hpp:234
KDL::FrameVel::operator==
IMETHOD friend bool operator==(const FrameVel &a, const FrameVel &b)
KDL::VectorVel::dot
IMETHOD friend doubleVel dot(const VectorVel &lhs, const VectorVel &rhs)
KDL::posrandom
IMETHOD void posrandom(doubleVel &F)
Definition: framevel.hpp:69
KDL::RotationVel::DoRotY
IMETHOD void DoRotY(const doubleVel &angle)
Definition: framevel.inl:212
KDL::TwistVel
Definition: framevel.hpp:269
KDL::TwistVel::RefPoint
IMETHOD TwistVel RefPoint(const VectorVel &v_base_AB)
Definition: framevel.inl:517
KDL::TwistVel::SetToZero
IMETHOD friend void SetToZero(TwistVel &v)
KDL::RotationVel::R
Rotation R
Definition: framevel.hpp:153
std::hash< KDL::doubleVel >::operator()
std::size_t operator()(KDL::doubleVel const &dv) const noexcept
Definition: framevel.hpp:417
KDL::RotationVel::Inverse
IMETHOD RotationVel Inverse() const
Definition: framevel.inl:170
KDL::random
IMETHOD void random(doubleVel &F)
Definition: framevel.hpp:65
KDL::RotationVel::Rot
static IMETHOD RotationVel Rot(const Vector &rotvec, const doubleVel &angle)
Definition: framevel.inl:229
KDL::FrameVel::FrameVel
FrameVel(const Frame &_T)
Definition: framevel.hpp:223
KDL::FrameVel::Equal
IMETHOD friend bool Equal(const FrameVel &r1, const FrameVel &r2, double eps)
std::hash< KDL::FrameVel >::operator()
std::size_t operator()(KDL::FrameVel const &fv) const noexcept
Definition: framevel.hpp:450
std::hash< KDL::TwistVel >::operator()
std::size_t operator()(KDL::TwistVel const &tv) const noexcept
Definition: framevel.hpp:461
KDL::RotationVel::operator==
IMETHOD friend bool operator==(const RotationVel &r1, const RotationVel &r2)
KDL::FrameVel::FrameVel
FrameVel(const RotationVel &_M, const VectorVel &_p)
Definition: framevel.hpp:229
std::hash< KDL::VectorVel >::operator()
std::size_t operator()(KDL::VectorVel const &vv) const noexcept
Definition: framevel.hpp:428
utility.h
KDL::hash_combine
void hash_combine(std::size_t &seed, const T &v)
Combine hash of object v to the seed.
Definition: hash_combine.h:18
KDL::TwistVel::Equal
IMETHOD friend bool Equal(const TwistVel &a, const TwistVel &b, double eps)
Traits< KDL::doubleVel >::valueType
double valueType
Definition: framevel.hpp:58
KDL::VectorVel::operator==
IMETHOD friend bool operator==(const VectorVel &r1, const VectorVel &r2)
KDL::FrameVel::M
RotationVel M
Definition: framevel.hpp:218
KDL::FrameVel::operator*
IMETHOD VectorVel operator*(const VectorVel &arg) const
Definition: framevel.inl:46
rall1d.h
KDL::VectorVel::v
Vector v
Definition: framevel.hpp:92
KDL::TwistVel::deriv
Twist deriv() const
Definition: framevel.hpp:287
KDL::FrameVel::operator!=
IMETHOD friend bool operator!=(const FrameVel &a, const FrameVel &b)
KDL::VectorVel::VectorVel
VectorVel()
Definition: framevel.hpp:94
KDL::FrameVel::operator=
IMETHOD FrameVel & operator=(const Frame &arg)
Definition: framevel.inl:70
KDL::addDelta
IMETHOD Vector addDelta(const Vector &p_w_a, const Vector &p_w_da, double dt=1)
adds vector da to vector a. see also the corresponding diff() routine.
KDL::VectorVel::operator!=
IMETHOD friend bool operator!=(const VectorVel &r1, const VectorVel &r2)
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::FrameVel::Inverse
IMETHOD FrameVel Inverse() const
Definition: framevel.inl:65
KDL::RotationVel::Rot2
static IMETHOD RotationVel Rot2(const Vector &rotvec, const doubleVel &angle)
Definition: framevel.inl:238
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:162
KDL::TwistVel::operator-
IMETHOD friend TwistVel operator-(const TwistVel &lhs, const TwistVel &rhs)
KDL::epsilon
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
Definition: utility.cxx:21
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:723
KDL::VectorVel::operator+=
IMETHOD VectorVel & operator+=(const VectorVel &arg)
Definition: framevel.inl:335
KDL::RotationVel::RotX
static IMETHOD RotationVel RotX(const doubleVel &angle)
Definition: framevel.inl:208
KDL::TwistVel::ReverseSign
IMETHOD void ReverseSign()
Definition: framevel.inl:511
KDL::TwistVel::operator-=
IMETHOD TwistVel & operator-=(const TwistVel &arg)
Definition: framevel.inl:527
KDL::VectorVel::operator+
IMETHOD friend VectorVel operator+(const VectorVel &r1, const VectorVel &r2)
KDL::VectorVel::ReverseSign
IMETHOD void ReverseSign()
Definition: framevel.inl:349
KDL::TwistVel::operator!=
IMETHOD friend bool operator!=(const TwistVel &a, const TwistVel &b)
KDL::RotationVel::UnitZ
IMETHOD VectorVel UnitZ() const
Definition: framevel.inl:160
KDL::VectorVel::Norm
IMETHOD doubleVel Norm(double eps=epsilon) const
Definition: framevel.inl:353
KDL::VectorVel::operator/
IMETHOD friend VectorVel operator/(const VectorVel &r1, double r2)
KDL::RotationVel
Definition: framevel.hpp:146
KDL::TwistVel::Zero
static IMETHOD TwistVel Zero()
Definition: framevel.inl:505
KDL::doubleVel
Rall1d< double > doubleVel
Definition: framevel.hpp:55
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
KDL::FrameVel::GetFrame
IMETHOD Frame GetFrame() const
Definition: framevel.inl:120
KDL::TwistVel::operator+=
IMETHOD TwistVel & operator+=(const TwistVel &arg)
Definition: framevel.inl:534
Traits< KDL::doubleVel >::derivType
KDL::doubleVel derivType
Definition: framevel.hpp:59
KDL::FrameVel::value
Frame value() const
Definition: framevel.hpp:233
KDL::VectorVel::SetToZero
IMETHOD friend void SetToZero(VectorVel &v)
KDL::RotationVel::RotZ
static IMETHOD RotationVel RotZ(const doubleVel &angle)
Definition: framevel.inl:224
KDL::TwistVel::TwistVel
TwistVel(const Twist &p, const Twist &v)
Definition: framevel.hpp:281
KDL::VectorVel
Definition: framevel.hpp:84
KDL::TwistVel::rot
VectorVel rot
Definition: framevel.hpp:275
KDL::VectorVel::VectorVel
VectorVel(const Vector &_p)
Definition: framevel.hpp:96
KDL::FrameVel::p
VectorVel p
Definition: framevel.hpp:219
KDL::FrameVel::GetTwist
IMETHOD Twist GetTwist() const
Definition: framevel.inl:124
KDL::TwistVel::operator+
IMETHOD friend TwistVel operator+(const TwistVel &lhs, const TwistVel &rhs)
KDL::RotationVel::operator*
IMETHOD VectorVel operator*(const VectorVel &arg) const
Definition: framevel.inl:189
KDL::FrameVel::FrameVel
FrameVel()
Definition: framevel.hpp:221
KDL::RotationVel::value
Rotation value() const
Definition: framevel.hpp:161
KDL::RotationVel::Identity
static IMETHOD RotationVel Identity()
Definition: framevel.inl:166
KDL::RotationVel::DoRotZ
IMETHOD void DoRotZ(const doubleVel &angle)
Definition: framevel.inl:220
KDL::Rall1d
Definition: rall1d.h:71
KDL::Equal
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
KDL::FrameVel::Identity
static IMETHOD FrameVel Identity()
Definition: framevel.inl:28
KDL::FrameVel::FrameVel
FrameVel(const Frame &_T, const Twist &_t)
Definition: framevel.hpp:226
KDL::VectorVel::operator*
IMETHOD friend VectorVel operator*(const VectorVel &r1, const VectorVel &r2)
KDL::TwistVel::vel
VectorVel vel
Definition: framevel.hpp:274
KDL::VectorVel::p
Vector p
Definition: framevel.hpp:91
KDL::VectorVel::deriv
Vector deriv() const
Definition: framevel.hpp:99
KDL::RotationVel::deriv
Vector deriv() const
Definition: framevel.hpp:162
KDL::VectorVel::operator=
IMETHOD VectorVel & operator=(const VectorVel &arg)
Definition: framevel.inl:325
KDL::RotationVel::RotationVel
RotationVel(const Rotation &_R)
Definition: framevel.hpp:157
KDL::RotationVel::UnitY
IMETHOD VectorVel UnitY() const
Definition: framevel.inl:156
KDL::FrameVel
Definition: framevel.hpp:209
std::size_t
KDL::VectorVel::Zero
static IMETHOD VectorVel Zero()
Definition: framevel.inl:346
KDL::diff
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
traits.h
KDL::TwistVel::operator==
IMETHOD friend bool operator==(const TwistVel &a, const TwistVel &b)
KDL::TwistVel::GetTwistDot
IMETHOD Twist GetTwistDot() const
Definition: framevel.inl:659
KDL::RotationVel::DoRotX
IMETHOD void DoRotX(const doubleVel &angle)
Definition: framevel.inl:204
IMETHOD
#define IMETHOD
Definition: utility.h:41
KDL::RotationVel::UnitX
IMETHOD VectorVel UnitX() const
Definition: framevel.inl:152
KDL::RotationVel::operator=
IMETHOD RotationVel & operator=(const RotationVel &arg)
Definition: framevel.inl:141
KDL::VectorVel::Equal
IMETHOD friend bool Equal(const VectorVel &r1, const VectorVel &r2, double eps)
KDL::TwistVel::TwistVel
TwistVel()
Definition: framevel.hpp:279
Traits
Traits are traits classes to determine the type of a derivative of another type.
Definition: traits.h:38
KDL::TwistVel::TwistVel
TwistVel(const Twist &p)
Definition: framevel.hpp:282
KDL::TwistVel::value
Twist value() const
Definition: framevel.hpp:284
KDL::VectorVel::operator-
IMETHOD friend VectorVel operator-(const VectorVel &r1, const VectorVel &r2)
KDL::VectorVel::VectorVel
VectorVel(const Vector &_p, const Vector &_v)
Definition: framevel.hpp:95
std::hash
KDL::RotationVel::operator!=
IMETHOD friend bool operator!=(const RotationVel &r1, const RotationVel &r2)
framevel.inl
KDL::Rotation
represents rotations in 3 dimensional space.
Definition: frames.hpp:303
KDL::RotationVel::Equal
IMETHOD friend bool Equal(const RotationVel &r1, const RotationVel &r2, double eps)