KDL  1.4.0
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;
41 typedef Rall2d<double,double,double> doubleAcc;
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:
64  Vector p;
65  Vector v;
66  Vector dv;
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) {}
73  IMETHOD VectorAcc& operator = (const VectorAcc& arg);
74  IMETHOD VectorAcc& operator = (const Vector& arg);
75  IMETHOD VectorAcc& operator += (const VectorAcc& arg);
76  IMETHOD VectorAcc& operator -= (const VectorAcc& arg);
77  IMETHOD static VectorAcc Zero();
78  IMETHOD void ReverseSign();
79  IMETHOD doubleAcc Norm();
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 };
107 
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) {}
121  IMETHOD RotationAcc& operator = (const RotationAcc& arg);
122  IMETHOD RotationAcc& operator = (const Rotation& arg);
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:
168  RotationAcc M;
169  VectorAcc p;
170 public:
171  FrameAcc(){}
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 };
200 
201 
202 
203 
204 
205 
206 
207 
208 //very similar to Wrench class.
209 class TwistAcc
210 {
211 public:
212  VectorAcc vel;
213  VectorAcc rot;
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);
221 
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);
225 
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);
229 
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();
237 
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
246 
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 };
259 
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
KDL::TwistAcc::GetTwist
IMETHOD Twist GetTwist() const
Definition: frameacc.inl:578
KDL::VectorAcc::operator+=
IMETHOD VectorAcc & operator+=(const VectorAcc &arg)
Definition: frameacc.inl:106
KDL::VectorAcc::operator-
IMETHOD friend VectorAcc operator-(const VectorAcc &r1, const VectorAcc &r2)
KDL::TwistAcc::operator+=
IMETHOD TwistAcc & operator+=(const TwistAcc &arg)
Definition: frameacc.inl:463
frames.hpp
KDL::TwistAcc::rot
VectorAcc rot
rotational velocity and its 1st and 2nd derivative
Definition: frameacc.hpp:239
KDL::TwistAcc::Zero
static IMETHOD TwistAcc Zero()
Definition: frameacc.inl:434
KDL::TwistAcc::operator+
IMETHOD friend TwistAcc operator+(const TwistAcc &lhs, const TwistAcc &rhs)
KDL::TwistAcc::operator-
IMETHOD friend TwistAcc operator-(const TwistAcc &lhs, const TwistAcc &rhs)
KDL::TwistAcc::SetToZero
IMETHOD friend void SetToZero(TwistAcc &v)
KDL::RotationAcc::operator*
IMETHOD VectorAcc operator*(const VectorAcc &arg) const
Definition: frameacc.inl:245
KDL::RotationAcc::Inverse
IMETHOD RotationAcc Inverse() const
Definition: frameacc.inl:224
KDL::VectorAcc::dot
IMETHOD friend doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
KDL::TwistAcc::GetTwistDot
IMETHOD Twist GetTwistDot() const
Definition: frameacc.inl:582
KDL::FrameAcc::Equal
IMETHOD friend bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps)
KDL::FrameAcc::operator=
IMETHOD FrameAcc & operator=(const FrameAcc &arg)
Definition: frameacc.inl:338
KDL::FrameAcc
Definition: frameacc.hpp:192
KDL::VectorAcc::VectorAcc
VectorAcc()
Definition: frameacc.hpp:94
KDL::RotationAcc::w
Vector w
angular velocity vector
Definition: frameacc.hpp:140
KDL::RotationAcc
Definition: frameacc.hpp:137
KDL
Definition: articulatedbodyinertia.cpp:28
KDL::VectorAcc::operator*
IMETHOD friend VectorAcc operator*(const VectorAcc &r1, const VectorAcc &r2)
KDL::VectorAcc::p
Vector p
position vector
Definition: frameacc.hpp:90
KDL::TwistAcc::operator-=
IMETHOD TwistAcc & operator-=(const TwistAcc &arg)
Definition: frameacc.inl:456
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:161
KDL::RotationAcc::R
Rotation R
rotation matrix
Definition: frameacc.hpp:139
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:720
KDL::RotationAcc::RotationAcc
RotationAcc()
Definition: frameacc.hpp:143
KDL::VectorAcc::v
Vector v
velocity vector
Definition: frameacc.hpp:91
KDL::TwistAcc::RefPoint
IMETHOD TwistAcc RefPoint(const VectorAcc &v_base_AB)
Definition: frameacc.inl:446
KDL::TwistAcc
Definition: frameacc.hpp:236
KDL::FrameAcc::p
VectorAcc p
Translation, velocity and acceleration of origin.
Definition: frameacc.hpp:195
KDL::VectorAcc::operator/
IMETHOD friend VectorAcc operator/(const VectorAcc &r1, double r2)
KDL::FrameAcc::GetFrame
IMETHOD Frame GetFrame() const
Definition: frameacc.inl:404
KDL::Frame
Definition: frames.hpp:570
KDL::VectorAcc::Zero
static IMETHOD VectorAcc Zero()
Definition: frameacc.inl:119
KDL::FrameAcc::GetAccTwist
IMETHOD Twist GetAccTwist() const
Definition: frameacc.inl:414
KDL::TwistAcc::TwistAcc
TwistAcc()
Definition: frameacc.hpp:242
KDL::FrameAcc::Identity
static IMETHOD FrameAcc Identity()
Definition: frameacc.inl:344
KDL::FrameAcc::GetTwist
IMETHOD Twist GetTwist() const
Definition: frameacc.inl:409
KDL::FrameAcc::FrameAcc
FrameAcc()
Definition: frameacc.hpp:197
KDL::VectorAcc::operator=
IMETHOD VectorAcc & operator=(const VectorAcc &arg)
Definition: frameacc.inl:92
KDL::TwistAcc::Equal
IMETHOD friend bool Equal(const TwistAcc &a, const TwistAcc &b, double eps)
KDL::RotationAcc::dw
Vector dw
angular acceration vector
Definition: frameacc.hpp:141
KDL::VectorAcc
Definition: frameacc.hpp:88
KDL::FrameAcc::Inverse
IMETHOD FrameAcc Inverse() const
Definition: frameacc.inl:381
KDL::RotationAcc::Identity
static IMETHOD RotationAcc Identity()
Definition: frameacc.inl:220
KDL::VectorAcc::dv
Vector dv
acceleration vector
Definition: frameacc.hpp:92
KDL::RotationAcc::Equal
IMETHOD friend bool Equal(const RotationAcc &r1, const RotationAcc &r2, double eps)
KDL::Equal
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
KDL::VectorAcc::ReverseSign
IMETHOD void ReverseSign()
Definition: frameacc.inl:123
KDL::VectorAcc::operator+
IMETHOD friend VectorAcc operator+(const VectorAcc &r1, const VectorAcc &r2)
KDL::FrameAcc::operator*
IMETHOD VectorAcc operator*(const VectorAcc &arg) const
Definition: frameacc.inl:362
KDL::doubleAcc
Rall2d< double, double, double > doubleAcc
Definition: frameacc.hpp:66
KDL::TwistAcc::operator/
IMETHOD friend TwistAcc operator/(const TwistAcc &lhs, double rhs)
frameacc.hpp
KDL::VectorAcc::Norm
IMETHOD doubleAcc Norm()
Definition: frameacc.inl:129
KDL::VectorAcc::Equal
IMETHOD friend bool Equal(const VectorAcc &r1, const VectorAcc &r2, double eps)
KDL::RotationAcc::operator=
IMETHOD RotationAcc & operator=(const RotationAcc &arg)
Definition: frameacc.inl:207
KDL::TwistAcc::vel
VectorAcc vel
translational velocity and its 1st and 2nd derivative
Definition: frameacc.hpp:238
frameacc.inl
KDL::FrameAcc::M
RotationAcc M
Rotation,angular velocity, and angular acceleration of frame.
Definition: frameacc.hpp:194
KDL::VectorAcc::operator-=
IMETHOD VectorAcc & operator-=(const VectorAcc &arg)
Definition: frameacc.inl:112
KDL::TwistAcc::operator*
IMETHOD friend TwistAcc operator*(const TwistAcc &lhs, double rhs)
KDL::TwistAcc::ReverseSign
IMETHOD void ReverseSign()
Definition: frameacc.inl:440
KDL::Rotation
represents rotations in 3 dimensional space.
Definition: frames.hpp:302