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"
30 #include "frames.hpp"
31
32
33 namespace KDL {
36
37 IMETHOD doubleVel diff(const doubleVel& a,const doubleVel& b,double dt=1.0) {
39 }
40
41 IMETHOD doubleVel addDelta(const doubleVel& a,const doubleVel&da,double dt=1.0) {
43 }
44
45 IMETHOD void random(doubleVel& F) {
46  random(F.t);
48 }
49 IMETHOD void posrandom(doubleVel& F) {
50  posrandom(F.t);
52 }
53
54 }
55
56 template <>
57 struct Traits<KDL::doubleVel> {
58  typedef double valueType;
60 };
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);
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);
103  IMETHOD VectorVel& operator += (const VectorVel& arg);
104  IMETHOD VectorVel& operator -= (const VectorVel& 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);
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
165  IMETHOD RotationVel& operator = (const RotationVel& arg);
166  IMETHOD RotationVel& operator = (const Rotation& arg);
167  IMETHOD VectorVel UnitX() const;
168  IMETHOD VectorVel UnitY() const;
169  IMETHOD VectorVel UnitZ() const;
170  IMETHOD static RotationVel Identity();
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) {}
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 };
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();
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
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);
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) {
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) {
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 }
365 IMETHOD FrameVel addDelta(const FrameVel& a,const TwistVel& da,double dt=1.0) {
366  return FrameVel(
369  );
370 }
371
373  random(a.p);
374  random(a.v);
375 }
377  random(a.vel);
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);
393 }
395  posrandom(a.vel);
396  posrandom(a.rot);
397 }
398
401  posrandom(R.w);
402 }
403
406  posrandom(F.p);
407 }
409 #ifdef KDL_INLINE
410 #include "framevel.inl"
411 #endif
412
413 } // namespace KDL
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 };
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 };
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 };
470 #endif
VectorAcc operator/(const VectorAcc &r1, double r2)
Definition: frameacc.hpp:181
represents rotations in 3 dimensional space.
Definition: frames.hpp:303
FrameVel(const RotationVel &_M, const VectorVel &_p)
Definition: framevel.hpp:229
VectorVel vel
Definition: framevel.hpp:274
VectorVel rot
Definition: framevel.hpp:275
std::size_t operator()(KDL::doubleVel const &dv) const noexcept
Definition: framevel.hpp:417
INLINE S Norm(const Rall1d< T, V, S > &value)
Definition: rall1d.h:418
TwistVel(const VectorVel &_vel, const VectorVel &_rot)
Definition: framevel.hpp:280
IMETHOD Rotation Rot(const Vector &axis_a_b)
Definition: frames.hpp:1108
FrameVel(const Frame &_T, const Twist &_t)
Definition: framevel.hpp:226
FrameVel(const Frame &_T)
Definition: framevel.hpp:223
ArticulatedBodyInertia operator+(const ArticulatedBodyInertia &Ia, const ArticulatedBodyInertia &Ib)
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
Definition: frames.hpp:1130
Frame value() const
Definition: framevel.hpp:233
Vector deriv() const
Definition: framevel.hpp:162
Traits are traits classes to determine the type of a derivative of another type.
Definition: traits.h:38
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
Definition: frameacc.hpp:138
RotationVel(const Rotation &_R, const Vector &_w)
Definition: framevel.hpp:158
std::size_t operator()(KDL::VectorVel const &vv) const noexcept
Definition: framevel.hpp:428
TwistVel(const Twist &p)
Definition: framevel.hpp:282
IMETHOD bool operator!=(const Frame &a, const Frame &b)
Definition: frames.hpp:1285
VectorVel(const Vector &_p, const Vector &_v)
Definition: framevel.hpp:95
std::size_t operator()(KDL::RotationVel const &rv) const noexcept
Definition: framevel.hpp:439
represents both translational and rotational velocities.
Definition: frames.hpp:723
IMETHOD void SetToZero(Vector &v)
Definition: frames.hpp:1069
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
Definition: frameacc.hpp:394
Rotation value() const
Definition: framevel.hpp:161
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:162
RotationVel M
Definition: framevel.hpp:218
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
Definition: utility.cxx:21
KDL::doubleVel derivType
Definition: framevel.hpp:59
Twist deriv() const
Definition: framevel.hpp:287
Vector deriv() const
Definition: framevel.hpp:99
ArticulatedBodyInertia operator*(double a, const ArticulatedBodyInertia &I)
IMETHOD void posrandom(Vector &a)
Definition: frames.hpp:1244
Vector value() const
Definition: framevel.hpp:98
std::size_t operator()(KDL::FrameVel const &fv) const noexcept
Definition: framevel.hpp:450
Definition: rall1d.h:57
T t
value
Definition: rall1d.h:56
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
VectorVel p
Definition: framevel.hpp:219
Twist deriv() const
Definition: framevel.hpp:234
IMETHOD Vector addDelta(const Vector &p_w_a, const Vector &p_w_da, double dt=1)
Definition: frames.hpp:1157
void hash_combine(std::size_t &seed, const T &v)
Combine hash of object v to the seed.
Definition: hash_combine.h:18
static IMETHOD RotationVel Identity()
Definition: framevel.hpp:167
Twist value() const
Definition: framevel.hpp:284
IMETHOD void random(Vector &a)
addDelta operator for displacement rotational velocity.
Definition: frames.hpp:1215
ArticulatedBodyInertia operator-(const ArticulatedBodyInertia &Ia, const ArticulatedBodyInertia &Ib)
#define IMETHOD
Definition: utility.h:41
bool operator==(const Rotation &a, const Rotation &b)
Definition: frames.cpp:430
Rall1d< double > doubleVel
Definition: framevel.hpp:35
IMETHOD RotationVel Inverse() const
Definition: framevel.hpp:171

orocos_kdl
Author(s):
autogenerated on Thu Apr 13 2023 02:19:14