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

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