KDL 1.5.1
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
38namespace KDL {
39
40class TwistAcc;
41typedef Rall2d<double,double,double> doubleAcc;
42
43// Equal is friend function, but default arguments for friends are forbidden (ยง8.3.6.4)
44class FrameAcc;
45class RotationAcc;
46class VectorAcc;
47
48IMETHOD bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps=epsilon);
49IMETHOD bool Equal(const Frame& r1,const FrameAcc& r2,double eps=epsilon);
50IMETHOD bool Equal(const FrameAcc& r1,const Frame& r2,double eps=epsilon);
51IMETHOD bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps=epsilon);
52IMETHOD bool Equal(const Rotation& r1,const RotationAcc& r2,double eps=epsilon);
53IMETHOD bool Equal(const RotationAcc& r1,const Rotation& r2,double eps=epsilon);
54IMETHOD bool Equal(const TwistAcc& a,const TwistAcc& b,double eps=epsilon);
55IMETHOD bool Equal(const Twist& a,const TwistAcc& b,double eps=epsilon);
56IMETHOD bool Equal(const TwistAcc& a,const Twist& b,double eps=epsilon);
57IMETHOD bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps=epsilon);
58IMETHOD bool Equal(const Vector& r1,const VectorAcc& r2,double eps=epsilon);
59IMETHOD bool Equal(const VectorAcc& r1,const Vector& r2,double eps=epsilon);
60
62{
63public:
67public:
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(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};
107
108
109
111{
112public:
116public:
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
166{
167public:
170public:
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.
210{
211public:
214public:
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
Definition: frameacc.hpp:166
IMETHOD FrameAcc & operator=(const FrameAcc &arg)
Definition: frameacc.inl:338
IMETHOD Twist GetTwist() const
Definition: frameacc.inl:409
IMETHOD FrameAcc Inverse() const
Definition: frameacc.inl:381
IMETHOD friend bool Equal(const FrameAcc &r1, const Frame &r2, double eps)
FrameAcc(const Frame &_T)
Definition: frameacc.hpp:172
static IMETHOD FrameAcc Identity()
Definition: frameacc.inl:344
RotationAcc M
Rotation,angular velocity, and angular acceleration of frame.
Definition: frameacc.hpp:168
IMETHOD Frame GetFrame() const
Definition: frameacc.inl:404
IMETHOD friend bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps)
IMETHOD VectorAcc operator*(const VectorAcc &arg) const
Definition: frameacc.inl:362
FrameAcc(const Frame &_T, const Twist &_t, const Twist &_dt)
Definition: frameacc.hpp:173
VectorAcc p
Translation, velocity and acceleration of origin.
Definition: frameacc.hpp:169
FrameAcc()
Definition: frameacc.hpp:171
IMETHOD Twist GetAccTwist() const
Definition: frameacc.inl:414
FrameAcc(const RotationAcc &_M, const VectorAcc &_p)
Definition: frameacc.hpp:175
IMETHOD friend bool Equal(const Frame &r1, const FrameAcc &r2, double eps)
Definition: frames.hpp:570
Definition: frameacc.hpp:111
IMETHOD RotationAcc Inverse() const
Definition: frameacc.inl:224
static IMETHOD RotationAcc Identity()
Definition: frameacc.inl:220
IMETHOD RotationAcc & operator=(const RotationAcc &arg)
Definition: frameacc.inl:207
RotationAcc(const Rotation &_R)
Definition: frameacc.hpp:118
Vector dw
angular acceration vector
Definition: frameacc.hpp:115
Vector w
angular velocity vector
Definition: frameacc.hpp:114
IMETHOD friend bool Equal(const Rotation &r1, const RotationAcc &r2, double eps)
IMETHOD friend bool Equal(const RotationAcc &r1, const Rotation &r2, double eps)
RotationAcc(const Rotation &_R, const Vector &_w, const Vector &_dw)
Definition: frameacc.hpp:119
IMETHOD friend bool Equal(const RotationAcc &r1, const RotationAcc &r2, double eps)
IMETHOD VectorAcc operator*(const VectorAcc &arg) const
Definition: frameacc.inl:245
RotationAcc()
Definition: frameacc.hpp:117
Rotation R
rotation matrix
Definition: frameacc.hpp:113
represents rotations in 3 dimensional space.
Definition: frames.hpp:302
Definition: frameacc.hpp:210
IMETHOD friend bool Equal(const Twist &a, const TwistAcc &b, double eps)
IMETHOD friend TwistAcc operator+(const TwistAcc &lhs, const TwistAcc &rhs)
IMETHOD friend TwistAcc operator*(const doubleAcc &lhs, const TwistAcc &rhs)
IMETHOD friend TwistAcc operator-(const TwistAcc &lhs, const TwistAcc &rhs)
IMETHOD friend TwistAcc operator-(const TwistAcc &arg)
VectorAcc rot
rotational velocity and its 1st and 2nd derivative
Definition: frameacc.hpp:213
IMETHOD friend bool Equal(const TwistAcc &a, const TwistAcc &b, double eps)
IMETHOD TwistAcc & operator+=(const TwistAcc &arg)
Definition: frameacc.inl:463
IMETHOD friend TwistAcc operator*(const TwistAcc &lhs, double rhs)
IMETHOD friend void SetToZero(TwistAcc &v)
IMETHOD void ReverseSign()
Definition: frameacc.inl:440
IMETHOD friend TwistAcc operator/(const TwistAcc &lhs, const doubleAcc &rhs)
IMETHOD friend TwistAcc operator*(double lhs, const TwistAcc &rhs)
static IMETHOD TwistAcc Zero()
Definition: frameacc.inl:434
IMETHOD friend bool Equal(const TwistAcc &a, const Twist &b, double eps)
IMETHOD TwistAcc & operator-=(const TwistAcc &arg)
Definition: frameacc.inl:456
IMETHOD Twist GetTwistDot() const
Definition: frameacc.inl:582
TwistAcc()
Definition: frameacc.hpp:216
IMETHOD TwistAcc RefPoint(const VectorAcc &v_base_AB)
Definition: frameacc.inl:446
IMETHOD Twist GetTwist() const
Definition: frameacc.inl:578
TwistAcc(const VectorAcc &_vel, const VectorAcc &_rot)
Definition: frameacc.hpp:217
VectorAcc vel
translational velocity and its 1st and 2nd derivative
Definition: frameacc.hpp:212
IMETHOD friend TwistAcc operator*(const TwistAcc &lhs, const doubleAcc &rhs)
IMETHOD friend TwistAcc operator/(const TwistAcc &lhs, double rhs)
represents both translational and rotational velocities.
Definition: frames.hpp:720
Definition: frameacc.hpp:62
IMETHOD friend bool Equal(const VectorAcc &r1, const Vector &r2, double eps)
Vector p
position vector
Definition: frameacc.hpp:64
IMETHOD friend VectorAcc operator/(const VectorAcc &r1, double r2)
VectorAcc(const Vector &_p)
Definition: frameacc.hpp:69
IMETHOD friend VectorAcc operator+(const VectorAcc &r1, const VectorAcc &r2)
IMETHOD friend doubleAcc dot(const Vector &lhs, const VectorAcc &rhs)
IMETHOD friend VectorAcc operator*(const Rotation &R, const VectorAcc &x)
IMETHOD friend doubleAcc dot(const VectorAcc &lhs, const Vector &rhs)
Vector dv
acceleration vector
Definition: frameacc.hpp:66
VectorAcc()
Definition: frameacc.hpp:68
IMETHOD VectorAcc & operator-=(const VectorAcc &arg)
Definition: frameacc.inl:112
Vector v
velocity vector
Definition: frameacc.hpp:65
IMETHOD VectorAcc & operator+=(const VectorAcc &arg)
Definition: frameacc.inl:106
IMETHOD friend bool Equal(const VectorAcc &r1, const VectorAcc &r2, double eps)
IMETHOD void ReverseSign()
Definition: frameacc.inl:123
IMETHOD friend bool Equal(const Vector &r1, const VectorAcc &r2, double eps)
VectorAcc(const Vector &_p, const Vector &_v)
Definition: frameacc.hpp:70
IMETHOD friend doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
IMETHOD friend VectorAcc operator-(const VectorAcc &r1, const VectorAcc &r2)
VectorAcc(const Vector &_p, const Vector &_v, const Vector &_dv)
Definition: frameacc.hpp:71
IMETHOD doubleAcc Norm(double eps=epsilon)
Definition: frameacc.inl:129
IMETHOD VectorAcc & operator=(const VectorAcc &arg)
Definition: frameacc.inl:92
static IMETHOD VectorAcc Zero()
Definition: frameacc.inl:119
IMETHOD friend VectorAcc operator*(const VectorAcc &r1, const VectorAcc &r2)
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:161
Definition: articulatedbodyinertia.cpp:26
Rall2d< double, double, double > doubleAcc
Definition: frameacc.hpp:41
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)