orocos_kdl
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 namespace KDL {
38 
39 class TwistAcc;
40 typedef Rall2d<double,double,double> doubleAcc;
41 
42 // Equal is friend function, but default arguments for friends are forbidden (ยง8.3.6.4)
43 class FrameAcc;
44 class RotationAcc;
45 class VectorAcc;
46 
47 IMETHOD bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps=epsilon);
48 IMETHOD bool Equal(const Frame& r1,const FrameAcc& r2,double eps=epsilon);
49 IMETHOD bool Equal(const FrameAcc& r1,const Frame& r2,double eps=epsilon);
50 IMETHOD bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps=epsilon);
51 IMETHOD bool Equal(const Rotation& r1,const RotationAcc& r2,double eps=epsilon);
52 IMETHOD bool Equal(const RotationAcc& r1,const Rotation& r2,double eps=epsilon);
53 IMETHOD bool Equal(const TwistAcc& a,const TwistAcc& b,double eps=epsilon);
54 IMETHOD bool Equal(const Twist& a,const TwistAcc& b,double eps=epsilon);
55 IMETHOD bool Equal(const TwistAcc& a,const Twist& b,double eps=epsilon);
56 IMETHOD bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps=epsilon);
57 IMETHOD bool Equal(const Vector& r1,const VectorAcc& r2,double eps=epsilon);
58 IMETHOD bool Equal(const VectorAcc& r1,const Vector& r2,double eps=epsilon);
59 
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(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 class RotationAcc
110 {
111 public:
112  Rotation R;
113  Vector w;
114  Vector dw;
115 public:
116  RotationAcc():R(),w() {}
117  explicit RotationAcc(const Rotation& _R):R(_R),w(Vector::Zero()){}
118  RotationAcc(const Rotation& _R,const Vector& _w,const Vector& _dw):
119  R(_R),w(_w),dw(_dw) {}
120  IMETHOD RotationAcc& operator = (const RotationAcc& arg);
121  IMETHOD RotationAcc& operator = (const Rotation& arg);
122  IMETHOD static RotationAcc Identity();
123  IMETHOD RotationAcc Inverse() const;
124  IMETHOD VectorAcc Inverse(const VectorAcc& arg) const;
125  IMETHOD VectorAcc Inverse(const Vector& arg) const;
126  IMETHOD VectorAcc operator*(const VectorAcc& arg) const;
127  IMETHOD VectorAcc operator*(const Vector& arg) const;
128 
129  // Rotations
130  // The SetRot.. functions set the value of *this to the appropriate rotation matrix.
131  // The Rot... static functions give the value of the appropriate rotation matrix back.
132  // The DoRot... functions apply a rotation R to *this,such that *this = *this * R.
133  // IMETHOD void DoRotX(const doubleAcc& angle);
134  // IMETHOD void DoRotY(const doubleAcc& angle);
135  // IMETHOD void DoRotZ(const doubleAcc& angle);
136  // IMETHOD static RRotation RotX(const doubleAcc& angle);
137  // IMETHOD static RRotation RotY(const doubleAcc& angle);
138  // IMETHOD static RRotation RotZ(const doubleAcc& angle);
139 
140  // IMETHOD void SetRot(const Vector& rotaxis,const doubleAcc& angle);
141  // Along an arbitrary axes. The norm of rotvec is neglected.
142  // IMETHOD static RotationAcc Rot(const Vector& rotvec,const doubleAcc& angle);
143  // rotvec has arbitrary norm
144  // rotation around a constant vector !
145  // IMETHOD static RotationAcc Rot2(const Vector& rotvec,const doubleAcc& angle);
146  // rotvec is normalized.
147  // rotation around a constant vector !
148 
149  IMETHOD friend RotationAcc operator* (const RotationAcc& r1,const RotationAcc& r2);
150  IMETHOD friend RotationAcc operator* (const Rotation& r1,const RotationAcc& r2);
151  IMETHOD friend RotationAcc operator* (const RotationAcc& r1,const Rotation& r2);
152  IMETHOD friend bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps);
153  IMETHOD friend bool Equal(const Rotation& r1,const RotationAcc& r2,double eps);
154  IMETHOD friend bool Equal(const RotationAcc& r1,const Rotation& r2,double eps);
155  IMETHOD TwistAcc Inverse(const TwistAcc& arg) const;
156  IMETHOD TwistAcc Inverse(const Twist& arg) const;
157  IMETHOD TwistAcc operator * (const TwistAcc& arg) const;
158  IMETHOD TwistAcc operator * (const Twist& arg) const;
159 };
160 
161 
162 class FrameAcc
163 {
164 public:
165  RotationAcc M;
166  VectorAcc p;
167 public:
168  FrameAcc(){}
169  explicit FrameAcc(const Frame& _T):M(_T.M),p(_T.p) {}
170  FrameAcc(const Frame& _T,const Twist& _t,const Twist& _dt):
171  M(_T.M,_t.rot,_dt.rot),p(_T.p,_t.vel,_dt.vel) {}
172  FrameAcc(const RotationAcc& _M,const VectorAcc& _p):M(_M),p(_p) {}
173 
174  IMETHOD FrameAcc& operator = (const FrameAcc& arg);
175  IMETHOD FrameAcc& operator = (const Frame& arg);
176  IMETHOD static FrameAcc Identity();
177  IMETHOD FrameAcc Inverse() const;
178  IMETHOD VectorAcc Inverse(const VectorAcc& arg) const;
179  IMETHOD VectorAcc operator*(const VectorAcc& arg) const;
180  IMETHOD VectorAcc operator*(const Vector& arg) const;
181  IMETHOD VectorAcc Inverse(const Vector& arg) const;
182  IMETHOD Frame GetFrame() const;
183  IMETHOD Twist GetTwist() const;
184  IMETHOD Twist GetAccTwist() const;
185  IMETHOD friend FrameAcc operator * (const FrameAcc& f1,const FrameAcc& f2);
186  IMETHOD friend FrameAcc operator * (const Frame& f1,const FrameAcc& f2);
187  IMETHOD friend FrameAcc operator * (const FrameAcc& f1,const Frame& f2);
188  IMETHOD friend bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps);
189  IMETHOD friend bool Equal(const Frame& r1,const FrameAcc& r2,double eps);
190  IMETHOD friend bool Equal(const FrameAcc& r1,const Frame& r2,double eps);
191 
192  IMETHOD TwistAcc Inverse(const TwistAcc& arg) const;
193  IMETHOD TwistAcc Inverse(const Twist& arg) const;
194  IMETHOD TwistAcc operator * (const TwistAcc& arg) const;
195  IMETHOD TwistAcc operator * (const Twist& arg) const;
196 };
197 
198 
199 //very similar to Wrench class.
200 class TwistAcc
201 {
202 public:
203  VectorAcc vel;
204  VectorAcc rot;
205 public:
206 
207  TwistAcc():vel(),rot() {};
208  TwistAcc(const VectorAcc& _vel,const VectorAcc& _rot):vel(_vel),rot(_rot) {};
209 
210  IMETHOD TwistAcc& operator-=(const TwistAcc& arg);
211  IMETHOD TwistAcc& operator+=(const TwistAcc& arg);
212 
213  IMETHOD friend TwistAcc operator*(const TwistAcc& lhs,double rhs);
214  IMETHOD friend TwistAcc operator*(double lhs,const TwistAcc& rhs);
215  IMETHOD friend TwistAcc operator/(const TwistAcc& lhs,double rhs);
216 
217  IMETHOD friend TwistAcc operator*(const TwistAcc& lhs,const doubleAcc& rhs);
218  IMETHOD friend TwistAcc operator*(const doubleAcc& lhs,const TwistAcc& rhs);
219  IMETHOD friend TwistAcc operator/(const TwistAcc& lhs,const doubleAcc& rhs);
220 
221  IMETHOD friend TwistAcc operator+(const TwistAcc& lhs,const TwistAcc& rhs);
222  IMETHOD friend TwistAcc operator-(const TwistAcc& lhs,const TwistAcc& rhs);
223  IMETHOD friend TwistAcc operator-(const TwistAcc& arg);
224 
225  IMETHOD friend void SetToZero(TwistAcc& v);
226 
227  static IMETHOD TwistAcc Zero();
228 
229  IMETHOD void ReverseSign();
230 
231  IMETHOD TwistAcc RefPoint(const VectorAcc& v_base_AB);
232  // Changes the reference point of the RTwist.
233  // The RVector v_base_AB is expressed in the same base as the RTwist
234  // The RVector v_base_AB is a RVector from the old point to
235  // the new point.
236  // Complexity : 6M+6A
237 
238  IMETHOD friend bool Equal(const TwistAcc& a,const TwistAcc& b,double eps);
239  IMETHOD friend bool Equal(const Twist& a,const TwistAcc& b,double eps);
240  IMETHOD friend bool Equal(const TwistAcc& a,const Twist& b,double eps);
241 
242 
243  IMETHOD Twist GetTwist() const;
244  IMETHOD Twist GetTwistDot() const;
245 
246  friend class RotationAcc;
247  friend class FrameAcc;
248 
249 };
250 
251 #ifdef KDL_INLINE
252 #include "frameacc.inl"
253 #endif
254 
255 } // namespace KDL
256 
257 template<> struct std::hash<KDL::doubleAcc>
258 {
259  std::size_t operator()(KDL::doubleAcc const& da) const noexcept
260  {
261  size_t seed = 0;
262  KDL::hash_combine(seed, da.t);
263  KDL::hash_combine(seed, da.d);
264  KDL::hash_combine(seed, da.dd);
265  return seed;
266  }
267 };
268 
269 template<> struct std::hash<KDL::VectorAcc>
270 {
271  std::size_t operator()(KDL::VectorAcc const& va) const noexcept
272  {
273  size_t seed = 0;
274  KDL::hash_combine(seed, va.p);
275  KDL::hash_combine(seed, va.v);
276  KDL::hash_combine(seed, va.dv);
277  return seed;
278  }
279 };
280 
281 template<> struct std::hash<KDL::RotationAcc>
282 {
283  std::size_t operator()(KDL::RotationAcc const& ra) const noexcept
284  {
285  size_t seed = 0;
286  KDL::hash_combine(seed, ra.R);
287  KDL::hash_combine(seed, ra.w);
288  KDL::hash_combine(seed, ra.dw);
289  return seed;
290  }
291 };
292 
293 template<> struct std::hash<KDL::FrameAcc>
294 {
295  std::size_t operator()(KDL::FrameAcc const& fa) const noexcept
296  {
297  size_t seed = 0;
298  KDL::hash_combine(seed, fa.M);
299  KDL::hash_combine(seed, fa.p);
300  return seed;
301  }
302 };
303 
304 template<> struct std::hash<KDL::TwistAcc>
305 {
306  std::size_t operator()(KDL::TwistAcc const& ta) const noexcept
307  {
308  size_t seed = 0;
309  KDL::hash_combine(seed, ta.vel);
310  KDL::hash_combine(seed, ta.rot);
311  return seed;
312  }
313 };
314 
315 #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:230
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)
std::hash< KDL::VectorAcc >::operator()
std::size_t operator()(KDL::VectorAcc const &va) const noexcept
Definition: frameacc.hpp:271
KDL::FrameAcc::operator=
IMETHOD FrameAcc & operator=(const FrameAcc &arg)
Definition: frameacc.inl:338
std::hash< KDL::FrameAcc >::operator()
std::size_t operator()(KDL::FrameAcc const &fa) const noexcept
Definition: frameacc.hpp:295
KDL::FrameAcc
Definition: frameacc.hpp:188
KDL::VectorAcc::VectorAcc
VectorAcc()
Definition: frameacc.hpp:94
KDL::RotationAcc::w
Vector w
angular velocity vector
Definition: frameacc.hpp:139
KDL::hash_combine
void hash_combine(std::size_t &seed, const T &v)
Combine hash of object v to the seed.
Definition: hash_combine.h:18
KDL::RotationAcc
Definition: frameacc.hpp:135
KDL
Definition: kukaLWR_DHnew.cpp:25
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:162
KDL::RotationAcc::R
Rotation R
rotation matrix
Definition: frameacc.hpp:138
KDL::epsilon
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
Definition: utility.cxx:21
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:723
KDL::RotationAcc::RotationAcc
RotationAcc()
Definition: frameacc.hpp:142
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:226
KDL::FrameAcc::p
VectorAcc p
Translation, velocity and acceleration of origin.
Definition: frameacc.hpp:192
KDL::VectorAcc::operator/
IMETHOD friend VectorAcc operator/(const VectorAcc &r1, double r2)
KDL::FrameAcc::GetFrame
IMETHOD Frame GetFrame() const
Definition: frameacc.inl:404
std::hash< KDL::RotationAcc >::operator()
std::size_t operator()(KDL::RotationAcc const &ra) const noexcept
Definition: frameacc.hpp:283
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
KDL::VectorAcc::Zero
static IMETHOD VectorAcc Zero()
Definition: frameacc.inl:119
KDL::FrameAcc::GetAccTwist
IMETHOD Twist GetAccTwist() const
Definition: frameacc.inl:414
KDL::Rall2d
Definition: rall2d.h:76
KDL::TwistAcc::TwistAcc
TwistAcc()
Definition: frameacc.hpp:233
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:194
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:140
KDL::VectorAcc
Definition: frameacc.hpp:87
KDL::FrameAcc::Inverse
IMETHOD FrameAcc Inverse() const
Definition: frameacc.inl:381
std::hash< KDL::doubleAcc >::operator()
std::size_t operator()(KDL::doubleAcc const &da) const noexcept
Definition: frameacc.hpp:259
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)
std::hash< KDL::TwistAcc >::operator()
std::size_t operator()(KDL::TwistAcc const &ta) const noexcept
Definition: frameacc.hpp:306
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:65
KDL::TwistAcc::operator/
IMETHOD friend TwistAcc operator/(const TwistAcc &lhs, double rhs)
std::size_t
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:229
frameacc.inl
IMETHOD
#define IMETHOD
Definition: utility.h:41
KDL::VectorAcc::Norm
IMETHOD doubleAcc Norm(double eps=epsilon)
Definition: frameacc.inl:129
KDL::FrameAcc::M
RotationAcc M
Rotation,angular velocity, and angular acceleration of frame.
Definition: frameacc.hpp:191
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)
std::hash
rall2d.h
KDL::TwistAcc::ReverseSign
IMETHOD void ReverseSign()
Definition: frameacc.inl:440
KDL::Rotation
represents rotations in 3 dimensional space.
Definition: frames.hpp:303
Generated on Tue Nov 26 2024 04:35:24 for orocos_kdl by   doxygen 1.8.17