00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #ifndef KDL_FRAMEVEL_H
00024 #define KDL_FRAMEVEL_H
00025
00026 #include "utilities/utility.h"
00027 #include "utilities/rall1d.h"
00028 #include "utilities/traits.h"
00029
00030 #include "frames.hpp"
00031
00032
00033
00034 namespace KDL {
00035
00036 typedef Rall1d<double> doubleVel;
00037
00038 IMETHOD doubleVel diff(const doubleVel& a,const doubleVel& b,double dt=1.0) {
00039 return doubleVel((b.t-a.t)/dt,(b.grad-a.grad)/dt);
00040 }
00041
00042 IMETHOD doubleVel addDelta(const doubleVel& a,const doubleVel&da,double dt=1.0) {
00043 return doubleVel(a.t+da.t*dt,a.grad+da.grad*dt);
00044 }
00045
00046 IMETHOD void random(doubleVel& F) {
00047 random(F.t);
00048 random(F.grad);
00049 }
00050 IMETHOD void posrandom(doubleVel& F) {
00051 posrandom(F.t);
00052 posrandom(F.grad);
00053 }
00054
00055 }
00056
00057 template <>
00058 struct Traits<KDL::doubleVel> {
00059 typedef double valueType;
00060 typedef KDL::doubleVel derivType;
00061 };
00062
00063 namespace KDL {
00064
00065 class TwistVel;
00066 class VectorVel;
00067 class FrameVel;
00068 class RotationVel;
00069
00070
00071 IMETHOD bool Equal(const VectorVel& r1,const VectorVel& r2,double eps=epsilon);
00072 IMETHOD bool Equal(const Vector& r1,const VectorVel& r2,double eps=epsilon);
00073 IMETHOD bool Equal(const VectorVel& r1,const Vector& r2,double eps=epsilon);
00074 IMETHOD bool Equal(const RotationVel& r1,const RotationVel& r2,double eps=epsilon);
00075 IMETHOD bool Equal(const Rotation& r1,const RotationVel& r2,double eps=epsilon);
00076 IMETHOD bool Equal(const RotationVel& r1,const Rotation& r2,double eps=epsilon);
00077 IMETHOD bool Equal(const FrameVel& r1,const FrameVel& r2,double eps=epsilon);
00078 IMETHOD bool Equal(const Frame& r1,const FrameVel& r2,double eps=epsilon);
00079 IMETHOD bool Equal(const FrameVel& r1,const Frame& r2,double eps=epsilon);
00080 IMETHOD bool Equal(const TwistVel& a,const TwistVel& b,double eps=epsilon);
00081 IMETHOD bool Equal(const Twist& a,const TwistVel& b,double eps=epsilon);
00082 IMETHOD bool Equal(const TwistVel& a,const Twist& b,double eps=epsilon);
00083
00084 class VectorVel
00085
00086
00087
00088
00089 {
00090 public:
00091 Vector p;
00092 Vector v;
00093 public:
00094 VectorVel():p(),v(){}
00095 VectorVel(const Vector& _p,const Vector& _v):p(_p),v(_v) {}
00096 explicit VectorVel(const Vector& _p):p(_p),v(Vector::Zero()) {}
00097
00098 Vector value() const { return p;}
00099 Vector deriv() const { return v;}
00100
00101 IMETHOD VectorVel& operator = (const VectorVel& arg);
00102 IMETHOD VectorVel& operator = (const Vector& arg);
00103 IMETHOD VectorVel& operator += (const VectorVel& arg);
00104 IMETHOD VectorVel& operator -= (const VectorVel& arg);
00105 IMETHOD static VectorVel Zero();
00106 IMETHOD void ReverseSign();
00107 IMETHOD doubleVel Norm() const;
00108 IMETHOD friend VectorVel operator + (const VectorVel& r1,const VectorVel& r2);
00109 IMETHOD friend VectorVel operator - (const VectorVel& r1,const VectorVel& r2);
00110 IMETHOD friend VectorVel operator + (const Vector& r1,const VectorVel& r2);
00111 IMETHOD friend VectorVel operator - (const Vector& r1,const VectorVel& r2);
00112 IMETHOD friend VectorVel operator + (const VectorVel& r1,const Vector& r2);
00113 IMETHOD friend VectorVel operator - (const VectorVel& r1,const Vector& r2);
00114 IMETHOD friend VectorVel operator * (const VectorVel& r1,const VectorVel& r2);
00115 IMETHOD friend VectorVel operator * (const VectorVel& r1,const Vector& r2);
00116 IMETHOD friend VectorVel operator * (const Vector& r1,const VectorVel& r2);
00117 IMETHOD friend VectorVel operator * (const VectorVel& r1,double r2);
00118 IMETHOD friend VectorVel operator * (double r1,const VectorVel& r2);
00119 IMETHOD friend VectorVel operator * (const doubleVel& r1,const VectorVel& r2);
00120 IMETHOD friend VectorVel operator * (const VectorVel& r2,const doubleVel& r1);
00121 IMETHOD friend VectorVel operator*(const Rotation& R,const VectorVel& x);
00122
00123 IMETHOD friend VectorVel operator / (const VectorVel& r1,double r2);
00124 IMETHOD friend VectorVel operator / (const VectorVel& r2,const doubleVel& r1);
00125 IMETHOD friend void SetToZero(VectorVel& v);
00126
00127
00128 IMETHOD friend bool Equal(const VectorVel& r1,const VectorVel& r2,double eps);
00129 IMETHOD friend bool Equal(const Vector& r1,const VectorVel& r2,double eps);
00130 IMETHOD friend bool Equal(const VectorVel& r1,const Vector& r2,double eps);
00131 IMETHOD friend VectorVel operator - (const VectorVel& r);
00132 IMETHOD friend doubleVel dot(const VectorVel& lhs,const VectorVel& rhs);
00133 IMETHOD friend doubleVel dot(const VectorVel& lhs,const Vector& rhs);
00134 IMETHOD friend doubleVel dot(const Vector& lhs,const VectorVel& rhs);
00135 };
00136
00137
00138
00139 class RotationVel
00140
00141
00142
00143
00144 {
00145 public:
00146 Rotation R;
00147 Vector w;
00148 public:
00149 RotationVel():R(),w() {}
00150 explicit RotationVel(const Rotation& _R):R(_R),w(Vector::Zero()){}
00151 RotationVel(const Rotation& _R,const Vector& _w):R(_R),w(_w){}
00152
00153
00154 Rotation value() const { return R;}
00155 Vector deriv() const { return w;}
00156
00157
00158 IMETHOD RotationVel& operator = (const RotationVel& arg);
00159 IMETHOD RotationVel& operator = (const Rotation& arg);
00160 IMETHOD VectorVel UnitX() const;
00161 IMETHOD VectorVel UnitY() const;
00162 IMETHOD VectorVel UnitZ() const;
00163 IMETHOD static RotationVel Identity();
00164 IMETHOD RotationVel Inverse() const;
00165 IMETHOD VectorVel Inverse(const VectorVel& arg) const;
00166 IMETHOD VectorVel Inverse(const Vector& arg) const;
00167 IMETHOD VectorVel operator*(const VectorVel& arg) const;
00168 IMETHOD VectorVel operator*(const Vector& arg) const;
00169 IMETHOD void DoRotX(const doubleVel& angle);
00170 IMETHOD void DoRotY(const doubleVel& angle);
00171 IMETHOD void DoRotZ(const doubleVel& angle);
00172 IMETHOD static RotationVel RotX(const doubleVel& angle);
00173 IMETHOD static RotationVel RotY(const doubleVel& angle);
00174 IMETHOD static RotationVel RotZ(const doubleVel& angle);
00175 IMETHOD static RotationVel Rot(const Vector& rotvec,const doubleVel& angle);
00176
00177
00178 IMETHOD static RotationVel Rot2(const Vector& rotvec,const doubleVel& angle);
00179
00180
00181 IMETHOD friend RotationVel operator* (const RotationVel& r1,const RotationVel& r2);
00182 IMETHOD friend RotationVel operator* (const Rotation& r1,const RotationVel& r2);
00183 IMETHOD friend RotationVel operator* (const RotationVel& r1,const Rotation& r2);
00184 IMETHOD friend bool Equal(const RotationVel& r1,const RotationVel& r2,double eps);
00185 IMETHOD friend bool Equal(const Rotation& r1,const RotationVel& r2,double eps);
00186 IMETHOD friend bool Equal(const RotationVel& r1,const Rotation& r2,double eps);
00187
00188 IMETHOD TwistVel Inverse(const TwistVel& arg) const;
00189 IMETHOD TwistVel Inverse(const Twist& arg) const;
00190 IMETHOD TwistVel operator * (const TwistVel& arg) const;
00191 IMETHOD TwistVel operator * (const Twist& arg) const;
00192 };
00193
00194
00195
00196
00197 class FrameVel
00198
00199
00200
00201
00202
00203
00204 {
00205 public:
00206 RotationVel M;
00207 VectorVel p;
00208 public:
00209 FrameVel(){}
00210
00211 explicit FrameVel(const Frame& _T):
00212 M(_T.M),p(_T.p) {}
00213
00214 FrameVel(const Frame& _T,const Twist& _t):
00215 M(_T.M,_t.rot),p(_T.p,_t.vel) {}
00216
00217 FrameVel(const RotationVel& _M,const VectorVel& _p):
00218 M(_M),p(_p) {}
00219
00220
00221 Frame value() const { return Frame(M.value(),p.value());}
00222 Twist deriv() const { return Twist(p.deriv(),M.deriv());}
00223
00224
00225 IMETHOD FrameVel& operator = (const Frame& arg);
00226 IMETHOD FrameVel& operator = (const FrameVel& arg);
00227 IMETHOD static FrameVel Identity();
00228 IMETHOD FrameVel Inverse() const;
00229 IMETHOD VectorVel Inverse(const VectorVel& arg) const;
00230 IMETHOD VectorVel operator*(const VectorVel& arg) const;
00231 IMETHOD VectorVel operator*(const Vector& arg) const;
00232 IMETHOD VectorVel Inverse(const Vector& arg) const;
00233 IMETHOD Frame GetFrame() const;
00234 IMETHOD Twist GetTwist() const;
00235 IMETHOD friend FrameVel operator * (const FrameVel& f1,const FrameVel& f2);
00236 IMETHOD friend FrameVel operator * (const Frame& f1,const FrameVel& f2);
00237 IMETHOD friend FrameVel operator * (const FrameVel& f1,const Frame& f2);
00238 IMETHOD friend bool Equal(const FrameVel& r1,const FrameVel& r2,double eps);
00239 IMETHOD friend bool Equal(const Frame& r1,const FrameVel& r2,double eps);
00240 IMETHOD friend bool Equal(const FrameVel& r1,const Frame& r2,double eps);
00241
00242 IMETHOD TwistVel Inverse(const TwistVel& arg) const;
00243 IMETHOD TwistVel Inverse(const Twist& arg) const;
00244 IMETHOD TwistVel operator * (const TwistVel& arg) const;
00245 IMETHOD TwistVel operator * (const Twist& arg) const;
00246 };
00247
00248
00249
00250
00251
00252
00253 class TwistVel
00254
00255
00256 {
00257 public:
00258 VectorVel vel;
00259 VectorVel rot;
00260 public:
00261
00262
00263 TwistVel():vel(),rot() {};
00264 TwistVel(const VectorVel& _vel,const VectorVel& _rot):vel(_vel),rot(_rot) {};
00265 TwistVel(const Twist& p,const Twist& v):vel(p.vel, v.vel), rot( p.rot, v.rot) {};
00266 TwistVel(const Twist& p):vel(p.vel), rot( p.rot) {};
00267
00268 Twist value() const {
00269 return Twist(vel.value(),rot.value());
00270 }
00271 Twist deriv() const {
00272 return Twist(vel.deriv(),rot.deriv());
00273 }
00274
00275 IMETHOD TwistVel& operator-=(const TwistVel& arg);
00276 IMETHOD TwistVel& operator+=(const TwistVel& arg);
00277
00278
00279 IMETHOD friend TwistVel operator*(const TwistVel& lhs,double rhs);
00280 IMETHOD friend TwistVel operator*(double lhs,const TwistVel& rhs);
00281 IMETHOD friend TwistVel operator/(const TwistVel& lhs,double rhs);
00282
00283 IMETHOD friend TwistVel operator*(const TwistVel& lhs,const doubleVel& rhs);
00284 IMETHOD friend TwistVel operator*(const doubleVel& lhs,const TwistVel& rhs);
00285 IMETHOD friend TwistVel operator/(const TwistVel& lhs,const doubleVel& rhs);
00286
00287 IMETHOD friend TwistVel operator+(const TwistVel& lhs,const TwistVel& rhs);
00288 IMETHOD friend TwistVel operator-(const TwistVel& lhs,const TwistVel& rhs);
00289 IMETHOD friend TwistVel operator-(const TwistVel& arg);
00290 IMETHOD friend void SetToZero(TwistVel& v);
00291
00292
00293
00294 static IMETHOD TwistVel Zero();
00295
00296
00297 IMETHOD void ReverseSign();
00298
00299
00300 IMETHOD TwistVel RefPoint(const VectorVel& v_base_AB);
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310 IMETHOD friend bool Equal(const TwistVel& a,const TwistVel& b,double eps);
00311 IMETHOD friend bool Equal(const Twist& a,const TwistVel& b,double eps);
00312 IMETHOD friend bool Equal(const TwistVel& a,const Twist& b,double eps);
00313
00314
00315 IMETHOD Twist GetTwist() const;
00316 IMETHOD Twist GetTwistDot() const;
00317
00318 friend class RotationVel;
00319 friend class FrameVel;
00320
00321 };
00322
00323 IMETHOD VectorVel diff(const VectorVel& a,const VectorVel& b,double dt=1.0) {
00324 return VectorVel(diff(a.p,b.p,dt),diff(a.v,b.v,dt));
00325 }
00326
00327 IMETHOD VectorVel addDelta(const VectorVel& a,const VectorVel&da,double dt=1.0) {
00328 return VectorVel(addDelta(a.p,da.p,dt),addDelta(a.v,da.v,dt));
00329 }
00330 IMETHOD VectorVel diff(const RotationVel& a,const RotationVel& b,double dt = 1.0) {
00331 return VectorVel(diff(a.R,b.R,dt),diff(a.w,b.w,dt));
00332 }
00333
00334 IMETHOD RotationVel addDelta(const RotationVel& a,const VectorVel&da,double dt=1.0) {
00335 return RotationVel(addDelta(a.R,da.p,dt),addDelta(a.w,da.v,dt));
00336 }
00337
00338 IMETHOD TwistVel diff(const FrameVel& a,const FrameVel& b,double dt=1.0) {
00339 return TwistVel(diff(a.M,b.M,dt),diff(a.p,b.p,dt));
00340 }
00341
00342 IMETHOD FrameVel addDelta(const FrameVel& a,const TwistVel& da,double dt=1.0) {
00343 return FrameVel(
00344 addDelta(a.M,da.rot,dt),
00345 addDelta(a.p,da.vel,dt)
00346 );
00347 }
00348
00349 IMETHOD void random(VectorVel& a) {
00350 random(a.p);
00351 random(a.v);
00352 }
00353 IMETHOD void random(TwistVel& a) {
00354 random(a.vel);
00355 random(a.rot);
00356 }
00357
00358 IMETHOD void random(RotationVel& R) {
00359 random(R.R);
00360 random(R.w);
00361 }
00362
00363 IMETHOD void random(FrameVel& F) {
00364 random(F.M);
00365 random(F.p);
00366 }
00367 IMETHOD void posrandom(VectorVel& a) {
00368 posrandom(a.p);
00369 posrandom(a.v);
00370 }
00371 IMETHOD void posrandom(TwistVel& a) {
00372 posrandom(a.vel);
00373 posrandom(a.rot);
00374 }
00375
00376 IMETHOD void posrandom(RotationVel& R) {
00377 posrandom(R.R);
00378 posrandom(R.w);
00379 }
00380
00381 IMETHOD void posrandom(FrameVel& F) {
00382 posrandom(F.M);
00383 posrandom(F.p);
00384 }
00385
00386 #ifdef KDL_INLINE
00387 #include "framevel.inl"
00388 #endif
00389
00390 }
00391
00392 #endif
00393
00394
00395
00396