frameacc.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 set (value,derivative,2nd derivative)
6  * and define operations on that set
7  * this classes are useful for automatic differentiation ( <-> symbolic diff ,
8  * <-> numeric diff).
9  * Defines VectorAcc, RotationAcc, FrameAcc, doubleAcc.
10  * Look at the corresponding classes Vector Rotation Frame Twist and
11  * Wrench for the semantics of the methods.
12  *
13  * It also contains the 2nd derivative <-> RFrames.h
14  *
15  * \author
16  * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
17  *
18  * \version
19  * ORO_Geometry V0.2
20  *
21  * \par History
22  * - $log$
23  *
24  * \par Release
25  * $Id: rrframes.h,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $
26  * $Name: $
27  ****************************************************************************/
28 
29 #ifndef RRFRAMES_H
30 #define RRFRAMES_H
31 
32 
33 #include "utilities/rall2d.h"
34 #include "frames.hpp"
35 
36 
37 
38 namespace KDL {
39 
40 class TwistAcc;
42 
43 // Equal is friend function, but default arguments for friends are forbidden (ยง8.3.6.4)
44 class FrameAcc;
45 class RotationAcc;
46 class VectorAcc;
47 
48 IMETHOD bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps=epsilon);
49 IMETHOD bool Equal(const Frame& r1,const FrameAcc& r2,double eps=epsilon);
50 IMETHOD bool Equal(const FrameAcc& r1,const Frame& r2,double eps=epsilon);
51 IMETHOD bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps=epsilon);
52 IMETHOD bool Equal(const Rotation& r1,const RotationAcc& r2,double eps=epsilon);
53 IMETHOD bool Equal(const RotationAcc& r1,const Rotation& r2,double eps=epsilon);
54 IMETHOD bool Equal(const TwistAcc& a,const TwistAcc& b,double eps=epsilon);
55 IMETHOD bool Equal(const Twist& a,const TwistAcc& b,double eps=epsilon);
56 IMETHOD bool Equal(const TwistAcc& a,const Twist& b,double eps=epsilon);
57 IMETHOD bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps=epsilon);
58 IMETHOD bool Equal(const Vector& r1,const VectorAcc& r2,double eps=epsilon);
59 IMETHOD bool Equal(const VectorAcc& r1,const Vector& r2,double eps=epsilon);
60 
61 class VectorAcc
62 {
63 public:
67 public:
68  VectorAcc():p(),v(),dv() {}
69  explicit VectorAcc(const Vector& _p):p(_p),v(Vector::Zero()),dv(Vector::Zero()) {}
70  VectorAcc(const Vector& _p,const Vector& _v):p(_p),v(_v),dv(Vector::Zero()) {}
71  VectorAcc(const Vector& _p,const Vector& _v,const Vector& _dv):
72  p(_p),v(_v),dv(_dv) {}
74  IMETHOD VectorAcc& operator = (const Vector& arg);
77  IMETHOD static VectorAcc Zero();
78  IMETHOD void ReverseSign();
79  IMETHOD doubleAcc Norm(double eps=epsilon);
80  IMETHOD friend VectorAcc operator + (const VectorAcc& r1,const VectorAcc& r2);
81  IMETHOD friend VectorAcc operator - (const VectorAcc& r1,const VectorAcc& r2);
82  IMETHOD friend VectorAcc operator + (const Vector& r1,const VectorAcc& r2);
83  IMETHOD friend VectorAcc operator - (const Vector& r1,const VectorAcc& r2);
84  IMETHOD friend VectorAcc operator + (const VectorAcc& r1,const Vector& r2);
85  IMETHOD friend VectorAcc operator - (const VectorAcc& r1,const Vector& r2);
86  IMETHOD friend VectorAcc operator * (const VectorAcc& r1,const VectorAcc& r2);
87  IMETHOD friend VectorAcc operator * (const VectorAcc& r1,const Vector& r2);
88  IMETHOD friend VectorAcc operator * (const Vector& r1,const VectorAcc& r2);
89  IMETHOD friend VectorAcc operator * (const VectorAcc& r1,double r2);
90  IMETHOD friend VectorAcc operator * (double r1,const VectorAcc& r2);
91  IMETHOD friend VectorAcc operator * (const doubleAcc& r1,const VectorAcc& r2);
92  IMETHOD friend VectorAcc operator * (const VectorAcc& r2,const doubleAcc& r1);
93  IMETHOD friend VectorAcc operator*(const Rotation& R,const VectorAcc& x);
94 
95  IMETHOD friend VectorAcc operator / (const VectorAcc& r1,double r2);
96  IMETHOD friend VectorAcc operator / (const VectorAcc& r2,const doubleAcc& r1);
97 
98 
99  IMETHOD friend bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps);
100  IMETHOD friend bool Equal(const Vector& r1,const VectorAcc& r2,double eps);
101  IMETHOD friend bool Equal(const VectorAcc& r1,const Vector& r2,double eps);
102  IMETHOD friend VectorAcc operator - (const VectorAcc& r);
103  IMETHOD friend doubleAcc dot(const VectorAcc& lhs,const VectorAcc& rhs);
104  IMETHOD friend doubleAcc dot(const VectorAcc& lhs,const Vector& rhs);
105  IMETHOD friend doubleAcc dot(const Vector& lhs,const VectorAcc& rhs);
106 };
108 
109 
111 {
112 public:
116 public:
117  RotationAcc():R(),w() {}
118  explicit RotationAcc(const Rotation& _R):R(_R),w(Vector::Zero()){}
119  RotationAcc(const Rotation& _R,const Vector& _w,const Vector& _dw):
120  R(_R),w(_w),dw(_dw) {}
123  IMETHOD static RotationAcc Identity();
124  IMETHOD RotationAcc Inverse() const;
125  IMETHOD VectorAcc Inverse(const VectorAcc& arg) const;
126  IMETHOD VectorAcc Inverse(const Vector& arg) const;
127  IMETHOD VectorAcc operator*(const VectorAcc& arg) const;
128  IMETHOD VectorAcc operator*(const Vector& arg) const;
129 
130  // Rotations
131  // The SetRot.. functions set the value of *this to the appropriate rotation matrix.
132  // The Rot... static functions give the value of the appropriate rotation matrix back.
133  // The DoRot... functions apply a rotation R to *this,such that *this = *this * R.
134  // IMETHOD void DoRotX(const doubleAcc& angle);
135  // IMETHOD void DoRotY(const doubleAcc& angle);
136  // IMETHOD void DoRotZ(const doubleAcc& angle);
137  // IMETHOD static RRotation RotX(const doubleAcc& angle);
138  // IMETHOD static RRotation RotY(const doubleAcc& angle);
139  // IMETHOD static RRotation RotZ(const doubleAcc& angle);
140 
141  // IMETHOD void SetRot(const Vector& rotaxis,const doubleAcc& angle);
142  // Along an arbitrary axes. The norm of rotvec is neglected.
143  // IMETHOD static RotationAcc Rot(const Vector& rotvec,const doubleAcc& angle);
144  // rotvec has arbitrary norm
145  // rotation around a constant vector !
146  // IMETHOD static RotationAcc Rot2(const Vector& rotvec,const doubleAcc& angle);
147  // rotvec is normalized.
148  // rotation around a constant vector !
149 
150  IMETHOD friend RotationAcc operator* (const RotationAcc& r1,const RotationAcc& r2);
151  IMETHOD friend RotationAcc operator* (const Rotation& r1,const RotationAcc& r2);
152  IMETHOD friend RotationAcc operator* (const RotationAcc& r1,const Rotation& r2);
153  IMETHOD friend bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps);
154  IMETHOD friend bool Equal(const Rotation& r1,const RotationAcc& r2,double eps);
155  IMETHOD friend bool Equal(const RotationAcc& r1,const Rotation& r2,double eps);
156  IMETHOD TwistAcc Inverse(const TwistAcc& arg) const;
157  IMETHOD TwistAcc Inverse(const Twist& arg) const;
158  IMETHOD TwistAcc operator * (const TwistAcc& arg) const;
159  IMETHOD TwistAcc operator * (const Twist& arg) const;
160 };
161 
162 
163 
164 
165 class FrameAcc
166 {
167 public:
170 public:
172  explicit FrameAcc(const Frame& _T):M(_T.M),p(_T.p) {}
173  FrameAcc(const Frame& _T,const Twist& _t,const Twist& _dt):
174  M(_T.M,_t.rot,_dt.rot),p(_T.p,_t.vel,_dt.vel) {}
175  FrameAcc(const RotationAcc& _M,const VectorAcc& _p):M(_M),p(_p) {}
176 
177  IMETHOD FrameAcc& operator = (const FrameAcc& arg);
178  IMETHOD FrameAcc& operator = (const Frame& arg);
179  IMETHOD static FrameAcc Identity();
180  IMETHOD FrameAcc Inverse() const;
181  IMETHOD VectorAcc Inverse(const VectorAcc& arg) const;
182  IMETHOD VectorAcc operator*(const VectorAcc& arg) const;
183  IMETHOD VectorAcc operator*(const Vector& arg) const;
184  IMETHOD VectorAcc Inverse(const Vector& arg) const;
185  IMETHOD Frame GetFrame() const;
186  IMETHOD Twist GetTwist() const;
187  IMETHOD Twist GetAccTwist() const;
188  IMETHOD friend FrameAcc operator * (const FrameAcc& f1,const FrameAcc& f2);
189  IMETHOD friend FrameAcc operator * (const Frame& f1,const FrameAcc& f2);
190  IMETHOD friend FrameAcc operator * (const FrameAcc& f1,const Frame& f2);
191  IMETHOD friend bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps);
192  IMETHOD friend bool Equal(const Frame& r1,const FrameAcc& r2,double eps);
193  IMETHOD friend bool Equal(const FrameAcc& r1,const Frame& r2,double eps);
194 
195  IMETHOD TwistAcc Inverse(const TwistAcc& arg) const;
196  IMETHOD TwistAcc Inverse(const Twist& arg) const;
197  IMETHOD TwistAcc operator * (const TwistAcc& arg) const;
198  IMETHOD TwistAcc operator * (const Twist& arg) const;
199 };
201 
202 
203 
205 
206 
207 
208 //very similar to Wrench class.
209 class TwistAcc
210 {
211 public:
214 public:
215 
216  TwistAcc():vel(),rot() {};
217  TwistAcc(const VectorAcc& _vel,const VectorAcc& _rot):vel(_vel),rot(_rot) {};
218 
219  IMETHOD TwistAcc& operator-=(const TwistAcc& arg);
220  IMETHOD TwistAcc& operator+=(const TwistAcc& arg);
222  IMETHOD friend TwistAcc operator*(const TwistAcc& lhs,double rhs);
223  IMETHOD friend TwistAcc operator*(double lhs,const TwistAcc& rhs);
224  IMETHOD friend TwistAcc operator/(const TwistAcc& lhs,double rhs);
226  IMETHOD friend TwistAcc operator*(const TwistAcc& lhs,const doubleAcc& rhs);
227  IMETHOD friend TwistAcc operator*(const doubleAcc& lhs,const TwistAcc& rhs);
228  IMETHOD friend TwistAcc operator/(const TwistAcc& lhs,const doubleAcc& rhs);
230  IMETHOD friend TwistAcc operator+(const TwistAcc& lhs,const TwistAcc& rhs);
231  IMETHOD friend TwistAcc operator-(const TwistAcc& lhs,const TwistAcc& rhs);
232  IMETHOD friend TwistAcc operator-(const TwistAcc& arg);
233 
234  IMETHOD friend void SetToZero(TwistAcc& v);
235 
236  static IMETHOD TwistAcc Zero();
238  IMETHOD void ReverseSign();
239 
240  IMETHOD TwistAcc RefPoint(const VectorAcc& v_base_AB);
241  // Changes the reference point of the RTwist.
242  // The RVector v_base_AB is expressed in the same base as the RTwist
243  // The RVector v_base_AB is a RVector from the old point to
244  // the new point.
245  // Complexity : 6M+6A
247  IMETHOD friend bool Equal(const TwistAcc& a,const TwistAcc& b,double eps);
248  IMETHOD friend bool Equal(const Twist& a,const TwistAcc& b,double eps);
249  IMETHOD friend bool Equal(const TwistAcc& a,const Twist& b,double eps);
250 
251 
252  IMETHOD Twist GetTwist() const;
253  IMETHOD Twist GetTwistDot() const;
254 
255  friend class RotationAcc;
256  friend class FrameAcc;
257 
258 };
260 
261 
262 
263 
264 
265 
266 #ifdef KDL_INLINE
267 #include "frameacc.inl"
268 #endif
269 
270 }
271 
272 
273 
274 
275 
276 #endif
RotationAcc M
Rotation,angular velocity, and angular acceleration of frame.
Definition: frameacc.hpp:168
IMETHOD void ReverseSign()
Definition: frameacc.hpp:124
IMETHOD friend VectorAcc operator/(const VectorAcc &r1, double r2)
Definition: frameacc.hpp:181
represents rotations in 3 dimensional space.
Definition: frames.hpp:301
Vector dw
angular acceration vector
Definition: frameacc.hpp:115
static IMETHOD VectorAcc Zero()
Definition: frameacc.hpp:120
Vector dv
acceleration vector
Definition: frameacc.hpp:66
Vector w
angular velocity vector
Definition: frameacc.hpp:114
FrameAcc(const Frame &_T, const Twist &_t, const Twist &_dt)
Definition: frameacc.hpp:173
IMETHOD VectorAcc & operator+=(const VectorAcc &arg)
Definition: frameacc.hpp:107
IMETHOD VectorAcc & operator=(const VectorAcc &arg)
Definition: frameacc.hpp:93
IMETHOD doubleAcc Norm(double eps=epsilon)
Definition: frameacc.hpp:130
VectorAcc rot
rotational velocity and its 1st and 2nd derivative
Definition: frameacc.hpp:213
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
VectorAcc(const Vector &_p, const Vector &_v)
Definition: frameacc.hpp:70
RotationAcc(const Rotation &_R, const Vector &_w, const Vector &_dw)
Definition: frameacc.hpp:119
IMETHOD friend VectorAcc operator-(const VectorAcc &r1, const VectorAcc &r2)
Definition: frameacc.hpp:29
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:160
Rotation R
rotation matrix
Definition: frameacc.hpp:113
IMETHOD friend bool Equal(const VectorAcc &r1, const VectorAcc &r2, double eps)
Definition: frameacc.hpp:160
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
RotationAcc(const Rotation &_R)
Definition: frameacc.hpp:118
Vector v
velocity vector
Definition: frameacc.hpp:65
IMETHOD friend VectorAcc operator+(const VectorAcc &r1, const VectorAcc &r2)
Definition: frameacc.hpp:25
Rall2d< double, double, double > doubleAcc
Definition: frameacc.hpp:40
Vector p
position vector
Definition: frameacc.hpp:64
VectorAcc(const Vector &_p)
Definition: frameacc.hpp:69
FrameAcc(const Frame &_T)
Definition: frameacc.hpp:172
FrameAcc(const RotationAcc &_M, const VectorAcc &_p)
Definition: frameacc.hpp:175
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
IMETHOD VectorAcc & operator-=(const VectorAcc &arg)
Definition: frameacc.hpp:113
VectorAcc p
Translation, velocity and acceleration of origin.
Definition: frameacc.hpp:169
VectorAcc vel
translational velocity and its 1st and 2nd derivative
Definition: frameacc.hpp:212
TwistAcc(const VectorAcc &_vel, const VectorAcc &_rot)
Definition: frameacc.hpp:217
IMETHOD friend VectorAcc operator*(const VectorAcc &r1, const VectorAcc &r2)
Definition: frameacc.hpp:53
#define IMETHOD
Definition: utility.h:41
IMETHOD friend doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
Definition: frameacc.hpp:138


orocos_kdl
Author(s):
autogenerated on Fri Mar 12 2021 03:05:43