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
34 namespace KDL {
35
37
38 IMETHOD doubleVel diff(const doubleVel& a,const doubleVel& b,double dt=1.0) {
40 }
41
42 IMETHOD doubleVel addDelta(const doubleVel& a,const doubleVel&da,double dt=1.0) {
44 }
45
46 IMETHOD void random(doubleVel& F) {
47  random(F.t);
49 }
50 IMETHOD void posrandom(doubleVel& F) {
53 }
54
55 }
57 template <>
58 struct Traits<KDL::doubleVel> {
59  typedef double valueType;
61 };
62
63 namespace KDL {
64
65 class TwistVel;
66 class VectorVel;
67 class FrameVel;
68 class RotationVel;
69
70 // Equal is friend function, but default arguments for friends are forbidden (ยง8.3.6.4)
71 IMETHOD bool Equal(const VectorVel& r1,const VectorVel& r2,double eps=epsilon);
72 IMETHOD bool Equal(const Vector& r1,const VectorVel& r2,double eps=epsilon);
73 IMETHOD bool Equal(const VectorVel& r1,const Vector& r2,double eps=epsilon);
74 IMETHOD bool Equal(const RotationVel& r1,const RotationVel& r2,double eps=epsilon);
75 IMETHOD bool Equal(const Rotation& r1,const RotationVel& r2,double eps=epsilon);
76 IMETHOD bool Equal(const RotationVel& r1,const Rotation& r2,double eps=epsilon);
77 IMETHOD bool Equal(const FrameVel& r1,const FrameVel& r2,double eps=epsilon);
78 IMETHOD bool Equal(const Frame& r1,const FrameVel& r2,double eps=epsilon);
79 IMETHOD bool Equal(const FrameVel& r1,const Frame& r2,double eps=epsilon);
80 IMETHOD bool Equal(const TwistVel& a,const TwistVel& b,double eps=epsilon);
81 IMETHOD bool Equal(const Twist& a,const TwistVel& b,double eps=epsilon);
82 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
146
148 // = TITLE
149 // An RotationVel is a Rotation and its first derivative, a rotation vector
150 // = CLASS TYPE
151 // Concrete
152 {
153 public:
154  Rotation R; // Rotation matrix
155  Vector w; // rotation vector
156 public:
157  RotationVel():R(),w() {}
158  explicit RotationVel(const Rotation& _R):R(_R),w(Vector::Zero()){}
159  RotationVel(const Rotation& _R,const Vector& _w):R(_R),w(_w){}
160
162  Rotation value() const { return R;}
163  Vector deriv() const { return w;}
164
165
166  IMETHOD RotationVel& operator = (const RotationVel& arg);
167  IMETHOD RotationVel& operator = (const Rotation& arg);
168  IMETHOD VectorVel UnitX() const;
169  IMETHOD VectorVel UnitY() const;
170  IMETHOD VectorVel UnitZ() const;
171  IMETHOD static RotationVel Identity();
172  IMETHOD RotationVel Inverse() const;
173  IMETHOD VectorVel Inverse(const VectorVel& arg) const;
174  IMETHOD VectorVel Inverse(const Vector& arg) const;
176  IMETHOD VectorVel operator*(const Vector& arg) const;
177  IMETHOD void DoRotX(const doubleVel& angle);
178  IMETHOD void DoRotY(const doubleVel& angle);
179  IMETHOD void DoRotZ(const doubleVel& angle);
180  IMETHOD static RotationVel RotX(const doubleVel& angle);
181  IMETHOD static RotationVel RotY(const doubleVel& angle);
182  IMETHOD static RotationVel RotZ(const doubleVel& angle);
183  IMETHOD static RotationVel Rot(const Vector& rotvec,const doubleVel& angle);
184  // rotvec has arbitrary norm
185  // rotation around a constant vector !
186  IMETHOD static RotationVel Rot2(const Vector& rotvec,const doubleVel& angle);
187  // rotvec is normalized.
188  // rotation around a constant vector !
189  IMETHOD friend RotationVel operator* (const RotationVel& r1,const RotationVel& r2);
190  IMETHOD friend RotationVel operator* (const Rotation& r1,const RotationVel& r2);
191  IMETHOD friend RotationVel operator* (const RotationVel& r1,const Rotation& r2);
192  IMETHOD friend bool Equal(const RotationVel& r1,const RotationVel& r2,double eps);
193  IMETHOD friend bool Equal(const Rotation& r1,const RotationVel& r2,double eps);
194  IMETHOD friend bool Equal(const RotationVel& r1,const Rotation& r2,double eps);
196  IMETHOD friend bool operator==(const RotationVel& r1,const RotationVel& r2);
197  IMETHOD friend bool operator!=(const RotationVel& r1,const RotationVel& r2);
198  IMETHOD friend bool operator==(const Rotation& r1,const RotationVel& r2);
199  IMETHOD friend bool operator!=(const Rotation& r1,const RotationVel& r2);
200  IMETHOD friend bool operator==(const RotationVel& r1,const Rotation& r2);
201  IMETHOD friend bool operator!=(const RotationVel& r1,const Rotation& r2);
202
203  IMETHOD TwistVel Inverse(const TwistVel& arg) const;
204  IMETHOD TwistVel Inverse(const Twist& arg) const;
206  IMETHOD TwistVel operator * (const Twist& arg) const;
207 };
208
210
211
212 class FrameVel
213 // = TITLE
214 // An FrameVel is a Frame and its first derivative, a Twist vector
215 // = CLASS TYPE
216 // Concrete
217 // = CAVEATS
218 //
219 {
220 public:
223 public:
226  explicit FrameVel(const Frame& _T):
227  M(_T.M),p(_T.p) {}
228
229  FrameVel(const Frame& _T,const Twist& _t):
230  M(_T.M,_t.rot),p(_T.p,_t.vel) {}
231
232  FrameVel(const RotationVel& _M,const VectorVel& _p):
233  M(_M),p(_p) {}
234
235
236  Frame value() const { return Frame(M.value(),p.value());}
237  Twist deriv() const { return Twist(p.deriv(),M.deriv());}
238
240  IMETHOD FrameVel& operator = (const Frame& arg);
241  IMETHOD FrameVel& operator = (const FrameVel& arg);
242  IMETHOD static FrameVel Identity();
243  IMETHOD FrameVel Inverse() const;
244  IMETHOD VectorVel Inverse(const VectorVel& arg) const;
245  IMETHOD VectorVel operator*(const VectorVel& arg) const;
246  IMETHOD VectorVel operator*(const Vector& arg) const;
247  IMETHOD VectorVel Inverse(const Vector& arg) const;
248  IMETHOD Frame GetFrame() const;
249  IMETHOD Twist GetTwist() const;
250  IMETHOD friend FrameVel operator * (const FrameVel& f1,const FrameVel& f2);
251  IMETHOD friend FrameVel operator * (const Frame& f1,const FrameVel& f2);
252  IMETHOD friend FrameVel operator * (const FrameVel& f1,const Frame& f2);
253  IMETHOD friend bool Equal(const FrameVel& r1,const FrameVel& r2,double eps);
254  IMETHOD friend bool Equal(const Frame& r1,const FrameVel& r2,double eps);
255  IMETHOD friend bool Equal(const FrameVel& r1,const Frame& r2,double eps);
256
257  IMETHOD friend bool operator==(const FrameVel& a,const FrameVel& b);
258  IMETHOD friend bool operator!=(const FrameVel& a,const FrameVel& b);
259  IMETHOD friend bool operator==(const Frame& a,const FrameVel& b);
260  IMETHOD friend bool operator!=(const Frame& a,const FrameVel& b);
261  IMETHOD friend bool operator==(const FrameVel& a,const Frame& b);
262  IMETHOD friend bool operator!=(const FrameVel& a,const Frame& b);
263
264  IMETHOD TwistVel Inverse(const TwistVel& arg) const;
265  IMETHOD TwistVel Inverse(const Twist& arg) const;
267  IMETHOD TwistVel operator * (const Twist& arg) const;
268 };
269
270
272
273
274 //very similar to Wrench class.
275 class TwistVel
276 // = TITLE
277 // This class represents a TwistVel. This is a velocity and rotational velocity together
278 {
279 public:
282 public:
283
284 // = Constructors
285  TwistVel():vel(),rot() {};
286  TwistVel(const VectorVel& _vel,const VectorVel& _rot):vel(_vel),rot(_rot) {};
287  TwistVel(const Twist& p,const Twist& v):vel(p.vel, v.vel), rot( p.rot, v.rot) {};
288  TwistVel(const Twist& p):vel(p.vel), rot( p.rot) {};
290  Twist value() const {
291  return Twist(vel.value(),rot.value());
292  }
293  Twist deriv() const {
294  return Twist(vel.deriv(),rot.deriv());
295  }
296 // = Operators
297  IMETHOD TwistVel& operator-=(const TwistVel& arg);
298  IMETHOD TwistVel& operator+=(const TwistVel& arg);
299
300 // = External operators
301  IMETHOD friend TwistVel operator*(const TwistVel& lhs,double rhs);
302  IMETHOD friend TwistVel operator*(double lhs,const TwistVel& rhs);
303  IMETHOD friend TwistVel operator/(const TwistVel& lhs,double rhs);
304
305  IMETHOD friend TwistVel operator*(const TwistVel& lhs,const doubleVel& rhs);
306  IMETHOD friend TwistVel operator*(const doubleVel& lhs,const TwistVel& rhs);
307  IMETHOD friend TwistVel operator/(const TwistVel& lhs,const doubleVel& rhs);
308
309  IMETHOD friend TwistVel operator+(const TwistVel& lhs,const TwistVel& rhs);
310  IMETHOD friend TwistVel operator-(const TwistVel& lhs,const TwistVel& rhs);
311  IMETHOD friend TwistVel operator-(const TwistVel& arg);
312  IMETHOD friend void SetToZero(TwistVel& v);
313
315 // = Zero
316  static IMETHOD TwistVel Zero();
317
318 // = Reverse Sign
319  IMETHOD void ReverseSign();
320
321 // = Change Reference point
322  IMETHOD TwistVel RefPoint(const VectorVel& v_base_AB);
323  // Changes the reference point of the TwistVel.
324  // The VectorVel v_base_AB is expressed in the same base as the TwistVel
325  // The VectorVel v_base_AB is a VectorVel from the old point to
326  // the new point.
327  // Complexity : 6M+6A
328
329  // = Equality operators
330  // do not use operator == because the definition of Equal(.,.) is slightly
331  // different. It compares whether the 2 arguments are equal in an eps-interval
332  IMETHOD friend bool Equal(const TwistVel& a,const TwistVel& b,double eps);
333  IMETHOD friend bool Equal(const Twist& a,const TwistVel& b,double eps);
334  IMETHOD friend bool Equal(const TwistVel& a,const Twist& b,double eps);
335
336  IMETHOD friend bool operator==(const TwistVel& a,const TwistVel& b);
337  IMETHOD friend bool operator!=(const TwistVel& a,const TwistVel& b);
338  IMETHOD friend bool operator==(const Twist& a,const TwistVel& b);
339  IMETHOD friend bool operator!=(const Twist& a,const TwistVel& b);
340  IMETHOD friend bool operator==(const TwistVel& a,const Twist& b);
341  IMETHOD friend bool operator!=(const TwistVel& a,const Twist& b);
342
343 // = Conversion to other entities
344  IMETHOD Twist GetTwist() const;
345  IMETHOD Twist GetTwistDot() const;
346 // = Friends
347  friend class RotationVel;
348  friend class FrameVel;
349
350 };
351
352 IMETHOD VectorVel diff(const VectorVel& a,const VectorVel& b,double dt=1.0) {
353  return VectorVel(diff(a.p,b.p,dt),diff(a.v,b.v,dt));
354 }
355
356 IMETHOD VectorVel addDelta(const VectorVel& a,const VectorVel&da,double dt=1.0) {
358 }
359 IMETHOD VectorVel diff(const RotationVel& a,const RotationVel& b,double dt = 1.0) {
360  return VectorVel(diff(a.R,b.R,dt),diff(a.w,b.w,dt));
361 }
362
363 IMETHOD RotationVel addDelta(const RotationVel& a,const VectorVel&da,double dt=1.0) {
365 }
366
367 IMETHOD TwistVel diff(const FrameVel& a,const FrameVel& b,double dt=1.0) {
368  return TwistVel(diff(a.M,b.M,dt),diff(a.p,b.p,dt));
369 }
371 IMETHOD FrameVel addDelta(const FrameVel& a,const TwistVel& da,double dt=1.0) {
372  return FrameVel(
375  );
376 }
377
379  random(a.p);
380  random(a.v);
381 }
383  random(a.vel);
384  random(a.rot);
385 }
386
388  random(R.R);
389  random(R.w);
390 }
391
393  random(F.M);
394  random(F.p);
395 }
397  posrandom(a.p);
398  posrandom(a.v);
399 }
401  posrandom(a.vel);
402  posrandom(a.rot);
403 }
404
406  posrandom(R.R);
407  posrandom(R.w);
408 }
409
412  posrandom(F.p);
413 }
415 #ifdef KDL_INLINE
416 #include "framevel.inl"
417 #endif
418
419 } // namespace
420
421 #endif
423
424
VectorAcc operator/(const VectorAcc &r1, double r2)
Definition: frameacc.hpp:181
represents rotations in 3 dimensional space.
Definition: frames.hpp:301
FrameVel(const RotationVel &_M, const VectorVel &_p)
Definition: framevel.hpp:232
VectorVel vel
Definition: framevel.hpp:280
VectorVel rot
Definition: framevel.hpp:281
INLINE S Norm(const Rall1d< T, V, S > &value)
Definition: rall1d.h:418
TwistVel(const VectorVel &_vel, const VectorVel &_rot)
Definition: framevel.hpp:286
IMETHOD Rotation Rot(const Vector &axis_a_b)
Definition: frames.hpp:1108
FrameVel(const Frame &_T, const Twist &_t)
Definition: framevel.hpp:229
FrameVel(const Frame &_T)
Definition: framevel.hpp:226
RotationVel(const Rotation &_R)
Definition: framevel.hpp:158
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:236
Vector deriv() const
Definition: framevel.hpp:163
Traits are traits classes to determine the type of a derivative of another type.
Definition: traits.h:38
TwistVel(const Twist &p, const Twist &v)
Definition: framevel.hpp:287
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
Definition: frameacc.hpp:138
RotationVel(const Rotation &_R, const Vector &_w)
Definition: framevel.hpp:159
TwistVel(const Twist &p)
Definition: framevel.hpp:288
IMETHOD bool operator!=(const Frame &a, const Frame &b)
Definition: frames.hpp:1285
VectorVel(const Vector &_p, const Vector &_v)
Definition: framevel.hpp:95
represents both translational and rotational velocities.
Definition: frames.hpp:720
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:162
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:160
RotationVel M
Definition: framevel.hpp:221
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
KDL::doubleVel derivType
Definition: framevel.hpp:60
Twist deriv() const
Definition: framevel.hpp:293
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
Definition: rall1d.h:57
IMETHOD RotationVel Inverse() const
Definition: framevel.hpp:171
T t
value
Definition: rall1d.h:56
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
VectorVel p
Definition: framevel.hpp:222
Twist deriv() const
Definition: framevel.hpp:237
IMETHOD Vector addDelta(const Vector &p_w_a, const Vector &p_w_da, double dt=1)
Definition: frames.hpp:1157
Twist value() const
Definition: framevel.hpp:290
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:36

orocos_kdl
Author(s):
autogenerated on Sat Mar 13 2021 03:16:37