00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #ifndef RRFRAMES_H
00030 #define RRFRAMES_H
00031
00032
00033 #include "utilities/rall2d.h"
00034 #include "frames.hpp"
00035
00036
00037
00038 namespace KDL {
00039
00040 class TwistAcc;
00041 typedef Rall2d<double,double,double> doubleAcc;
00042
00043
00044 class FrameAcc;
00045 class RotationAcc;
00046 class VectorAcc;
00047
00048 IMETHOD bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps=epsilon);
00049 IMETHOD bool Equal(const Frame& r1,const FrameAcc& r2,double eps=epsilon);
00050 IMETHOD bool Equal(const FrameAcc& r1,const Frame& r2,double eps=epsilon);
00051 IMETHOD bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps=epsilon);
00052 IMETHOD bool Equal(const Rotation& r1,const RotationAcc& r2,double eps=epsilon);
00053 IMETHOD bool Equal(const RotationAcc& r1,const Rotation& r2,double eps=epsilon);
00054 IMETHOD bool Equal(const TwistAcc& a,const TwistAcc& b,double eps=epsilon);
00055 IMETHOD bool Equal(const Twist& a,const TwistAcc& b,double eps=epsilon);
00056 IMETHOD bool Equal(const TwistAcc& a,const Twist& b,double eps=epsilon);
00057 IMETHOD bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps=epsilon);
00058 IMETHOD bool Equal(const Vector& r1,const VectorAcc& r2,double eps=epsilon);
00059 IMETHOD bool Equal(const VectorAcc& r1,const Vector& r2,double eps=epsilon);
00060
00061 class VectorAcc
00062 {
00063 public:
00064 Vector p;
00065 Vector v;
00066 Vector dv;
00067 public:
00068 VectorAcc():p(),v(),dv() {}
00069 explicit VectorAcc(const Vector& _p):p(_p),v(Vector::Zero()),dv(Vector::Zero()) {}
00070 VectorAcc(const Vector& _p,const Vector& _v):p(_p),v(_v),dv(Vector::Zero()) {}
00071 VectorAcc(const Vector& _p,const Vector& _v,const Vector& _dv):
00072 p(_p),v(_v),dv(_dv) {}
00073 IMETHOD VectorAcc& operator = (const VectorAcc& arg);
00074 IMETHOD VectorAcc& operator = (const Vector& arg);
00075 IMETHOD VectorAcc& operator += (const VectorAcc& arg);
00076 IMETHOD VectorAcc& operator -= (const VectorAcc& arg);
00077 IMETHOD static VectorAcc Zero();
00078 IMETHOD void ReverseSign();
00079 IMETHOD doubleAcc Norm();
00080 IMETHOD friend VectorAcc operator + (const VectorAcc& r1,const VectorAcc& r2);
00081 IMETHOD friend VectorAcc operator - (const VectorAcc& r1,const VectorAcc& r2);
00082 IMETHOD friend VectorAcc operator + (const Vector& r1,const VectorAcc& r2);
00083 IMETHOD friend VectorAcc operator - (const Vector& r1,const VectorAcc& r2);
00084 IMETHOD friend VectorAcc operator + (const VectorAcc& r1,const Vector& r2);
00085 IMETHOD friend VectorAcc operator - (const VectorAcc& r1,const Vector& r2);
00086 IMETHOD friend VectorAcc operator * (const VectorAcc& r1,const VectorAcc& r2);
00087 IMETHOD friend VectorAcc operator * (const VectorAcc& r1,const Vector& r2);
00088 IMETHOD friend VectorAcc operator * (const Vector& r1,const VectorAcc& r2);
00089 IMETHOD friend VectorAcc operator * (const VectorAcc& r1,double r2);
00090 IMETHOD friend VectorAcc operator * (double r1,const VectorAcc& r2);
00091 IMETHOD friend VectorAcc operator * (const doubleAcc& r1,const VectorAcc& r2);
00092 IMETHOD friend VectorAcc operator * (const VectorAcc& r2,const doubleAcc& r1);
00093 IMETHOD friend VectorAcc operator*(const Rotation& R,const VectorAcc& x);
00094
00095 IMETHOD friend VectorAcc operator / (const VectorAcc& r1,double r2);
00096 IMETHOD friend VectorAcc operator / (const VectorAcc& r2,const doubleAcc& r1);
00097
00098
00099 IMETHOD friend bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps);
00100 IMETHOD friend bool Equal(const Vector& r1,const VectorAcc& r2,double eps);
00101 IMETHOD friend bool Equal(const VectorAcc& r1,const Vector& r2,double eps);
00102 IMETHOD friend VectorAcc operator - (const VectorAcc& r);
00103 IMETHOD friend doubleAcc dot(const VectorAcc& lhs,const VectorAcc& rhs);
00104 IMETHOD friend doubleAcc dot(const VectorAcc& lhs,const Vector& rhs);
00105 IMETHOD friend doubleAcc dot(const Vector& lhs,const VectorAcc& rhs);
00106 };
00107
00108
00109
00110 class RotationAcc
00111 {
00112 public:
00113 Rotation R;
00114 Vector w;
00115 Vector dw;
00116 public:
00117 RotationAcc():R(),w() {}
00118 explicit RotationAcc(const Rotation& _R):R(_R),w(Vector::Zero()){}
00119 RotationAcc(const Rotation& _R,const Vector& _w,const Vector& _dw):
00120 R(_R),w(_w),dw(_dw) {}
00121 IMETHOD RotationAcc& operator = (const RotationAcc& arg);
00122 IMETHOD RotationAcc& operator = (const Rotation& arg);
00123 IMETHOD static RotationAcc Identity();
00124 IMETHOD RotationAcc Inverse() const;
00125 IMETHOD VectorAcc Inverse(const VectorAcc& arg) const;
00126 IMETHOD VectorAcc Inverse(const Vector& arg) const;
00127 IMETHOD VectorAcc operator*(const VectorAcc& arg) const;
00128 IMETHOD VectorAcc operator*(const Vector& arg) const;
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150 IMETHOD friend RotationAcc operator* (const RotationAcc& r1,const RotationAcc& r2);
00151 IMETHOD friend RotationAcc operator* (const Rotation& r1,const RotationAcc& r2);
00152 IMETHOD friend RotationAcc operator* (const RotationAcc& r1,const Rotation& r2);
00153 IMETHOD friend bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps);
00154 IMETHOD friend bool Equal(const Rotation& r1,const RotationAcc& r2,double eps);
00155 IMETHOD friend bool Equal(const RotationAcc& r1,const Rotation& r2,double eps);
00156 IMETHOD TwistAcc Inverse(const TwistAcc& arg) const;
00157 IMETHOD TwistAcc Inverse(const Twist& arg) const;
00158 IMETHOD TwistAcc operator * (const TwistAcc& arg) const;
00159 IMETHOD TwistAcc operator * (const Twist& arg) const;
00160 };
00161
00162
00163
00164
00165 class FrameAcc
00166 {
00167 public:
00168 RotationAcc M;
00169 VectorAcc p;
00170 public:
00171 FrameAcc(){}
00172 explicit FrameAcc(const Frame& _T):M(_T.M),p(_T.p) {}
00173 FrameAcc(const Frame& _T,const Twist& _t,const Twist& _dt):
00174 M(_T.M,_t.rot,_dt.rot),p(_T.p,_t.vel,_dt.vel) {}
00175 FrameAcc(const RotationAcc& _M,const VectorAcc& _p):M(_M),p(_p) {}
00176
00177 IMETHOD FrameAcc& operator = (const FrameAcc& arg);
00178 IMETHOD FrameAcc& operator = (const Frame& arg);
00179 IMETHOD static FrameAcc Identity();
00180 IMETHOD FrameAcc Inverse() const;
00181 IMETHOD VectorAcc Inverse(const VectorAcc& arg) const;
00182 IMETHOD VectorAcc operator*(const VectorAcc& arg) const;
00183 IMETHOD VectorAcc operator*(const Vector& arg) const;
00184 IMETHOD VectorAcc Inverse(const Vector& arg) const;
00185 IMETHOD Frame GetFrame() const;
00186 IMETHOD Twist GetTwist() const;
00187 IMETHOD Twist GetAccTwist() const;
00188 IMETHOD friend FrameAcc operator * (const FrameAcc& f1,const FrameAcc& f2);
00189 IMETHOD friend FrameAcc operator * (const Frame& f1,const FrameAcc& f2);
00190 IMETHOD friend FrameAcc operator * (const FrameAcc& f1,const Frame& f2);
00191 IMETHOD friend bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps);
00192 IMETHOD friend bool Equal(const Frame& r1,const FrameAcc& r2,double eps);
00193 IMETHOD friend bool Equal(const FrameAcc& r1,const Frame& r2,double eps);
00194
00195 IMETHOD TwistAcc Inverse(const TwistAcc& arg) const;
00196 IMETHOD TwistAcc Inverse(const Twist& arg) const;
00197 IMETHOD TwistAcc operator * (const TwistAcc& arg) const;
00198 IMETHOD TwistAcc operator * (const Twist& arg) const;
00199 };
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209 class TwistAcc
00210 {
00211 public:
00212 VectorAcc vel;
00213 VectorAcc rot;
00214 public:
00215
00216 TwistAcc():vel(),rot() {};
00217 TwistAcc(const VectorAcc& _vel,const VectorAcc& _rot):vel(_vel),rot(_rot) {};
00218
00219 IMETHOD TwistAcc& operator-=(const TwistAcc& arg);
00220 IMETHOD TwistAcc& operator+=(const TwistAcc& arg);
00221
00222 IMETHOD friend TwistAcc operator*(const TwistAcc& lhs,double rhs);
00223 IMETHOD friend TwistAcc operator*(double lhs,const TwistAcc& rhs);
00224 IMETHOD friend TwistAcc operator/(const TwistAcc& lhs,double rhs);
00225
00226 IMETHOD friend TwistAcc operator*(const TwistAcc& lhs,const doubleAcc& rhs);
00227 IMETHOD friend TwistAcc operator*(const doubleAcc& lhs,const TwistAcc& rhs);
00228 IMETHOD friend TwistAcc operator/(const TwistAcc& lhs,const doubleAcc& rhs);
00229
00230 IMETHOD friend TwistAcc operator+(const TwistAcc& lhs,const TwistAcc& rhs);
00231 IMETHOD friend TwistAcc operator-(const TwistAcc& lhs,const TwistAcc& rhs);
00232 IMETHOD friend TwistAcc operator-(const TwistAcc& arg);
00233
00234 IMETHOD friend void SetToZero(TwistAcc& v);
00235
00236 static IMETHOD TwistAcc Zero();
00237
00238 IMETHOD void ReverseSign();
00239
00240 IMETHOD TwistAcc RefPoint(const VectorAcc& v_base_AB);
00241
00242
00243
00244
00245
00246
00247 IMETHOD friend bool Equal(const TwistAcc& a,const TwistAcc& b,double eps);
00248 IMETHOD friend bool Equal(const Twist& a,const TwistAcc& b,double eps);
00249 IMETHOD friend bool Equal(const TwistAcc& a,const Twist& b,double eps);
00250
00251
00252 IMETHOD Twist GetTwist() const;
00253 IMETHOD Twist GetTwistDot() const;
00254
00255 friend class RotationAcc;
00256 friend class FrameAcc;
00257
00258 };
00259
00260
00261
00262
00263
00264
00265
00266 #ifdef KDL_INLINE
00267 #include "frameacc.inl"
00268 #endif
00269
00270 }
00271
00272
00273
00274
00275
00276 #endif