framevel.hpp
Go to the documentation of this file.
```00001 /*****************************************************************************
00002  * \file
00003  *      This file contains the definition of classes for a
00004  *      Rall Algebra of (subset of) the classes defined in frames,
00005  *      i.e. classes that contain a pair (value,derivative) and define operations on that pair
00006  *      this classes are usefull for automatic differentiation ( <-> symbolic diff , <-> numeric diff)
00007  *      Defines VectorVel, RotationVel, FrameVel.  Look at Frames.h for details on how to work
00008  *      with Frame objects.
00009  *  \author
00010  *      Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
00011  *
00012  *  \version
00013  *      ORO_Geometry V0.2
00014  *
00015  *  \par History
00016  *      - \$log\$
00017  *
00018  *  \par Release
00019  *      \$Id: rframes.h,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp \$
00020  *      \$Name:  \$
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 class VectorVel
00071 // = TITLE
00072 //     An VectorVel is a Vector and its first derivative
00073 // = CLASS TYPE
00074 //     Concrete
00075 {
00076 public:
00077     Vector p;       // position vector
00078     Vector v;       // velocity vector
00079 public:
00080     VectorVel():p(),v(){}
00081     VectorVel(const Vector& _p,const Vector& _v):p(_p),v(_v) {}
00082     explicit VectorVel(const Vector& _p):p(_p),v(Vector::Zero()) {}
00083
00084     Vector value() const { return p;}
00085     Vector deriv() const { return v;}
00086
00087     IMETHOD VectorVel& operator = (const VectorVel& arg);
00088     IMETHOD VectorVel& operator = (const Vector& arg);
00089     IMETHOD VectorVel& operator += (const VectorVel& arg);
00090     IMETHOD VectorVel& operator -= (const VectorVel& arg);
00091     IMETHOD static VectorVel Zero();
00092     IMETHOD void ReverseSign();
00093     IMETHOD doubleVel Norm() const;
00094     IMETHOD friend VectorVel operator + (const VectorVel& r1,const VectorVel& r2);
00095     IMETHOD friend VectorVel operator - (const VectorVel& r1,const VectorVel& r2);
00096     IMETHOD friend VectorVel operator + (const Vector& r1,const VectorVel& r2);
00097     IMETHOD friend VectorVel operator - (const Vector& r1,const VectorVel& r2);
00098     IMETHOD friend VectorVel operator + (const VectorVel& r1,const Vector& r2);
00099     IMETHOD friend VectorVel operator - (const VectorVel& r1,const Vector& r2);
00100     IMETHOD friend VectorVel operator * (const VectorVel& r1,const VectorVel& r2);
00101     IMETHOD friend VectorVel operator * (const VectorVel& r1,const Vector& r2);
00102     IMETHOD friend VectorVel operator * (const Vector& r1,const VectorVel& r2);
00103     IMETHOD friend VectorVel operator * (const VectorVel& r1,double r2);
00104     IMETHOD friend VectorVel operator * (double r1,const VectorVel& r2);
00105     IMETHOD friend VectorVel operator * (const doubleVel& r1,const VectorVel& r2);
00106     IMETHOD friend VectorVel operator * (const VectorVel& r2,const doubleVel& r1);
00107     IMETHOD friend VectorVel operator*(const Rotation& R,const VectorVel& x);
00108
00109     IMETHOD friend VectorVel operator / (const VectorVel& r1,double r2);
00110     IMETHOD friend VectorVel operator / (const VectorVel& r2,const doubleVel& r1);
00111     IMETHOD friend void SetToZero(VectorVel& v);
00112
00113
00114     IMETHOD friend bool Equal(const VectorVel& r1,const VectorVel& r2,double eps=epsilon);
00115     IMETHOD friend bool Equal(const Vector& r1,const VectorVel& r2,double eps=epsilon);
00116     IMETHOD friend bool Equal(const VectorVel& r1,const Vector& r2,double eps=epsilon);
00117     IMETHOD friend VectorVel operator - (const VectorVel& r);
00118     IMETHOD friend doubleVel dot(const VectorVel& lhs,const VectorVel& rhs);
00119     IMETHOD friend doubleVel dot(const VectorVel& lhs,const Vector& rhs);
00120     IMETHOD friend doubleVel dot(const Vector& lhs,const VectorVel& rhs);
00121 };
00122
00123
00124
00125 class RotationVel
00126 // = TITLE
00127 //     An RotationVel is a Rotation and its first derivative, a rotation vector
00128 // = CLASS TYPE
00129 //     Concrete
00130 {
00131 public:
00132     Rotation R; // Rotation matrix
00133     Vector   w; // rotation vector
00134 public:
00135     RotationVel():R(),w() {}
00136     explicit RotationVel(const Rotation& _R):R(_R),w(Vector::Zero()){}
00137     RotationVel(const Rotation& _R,const Vector& _w):R(_R),w(_w){}
00138
00139
00140     Rotation value() const { return R;}
00141     Vector   deriv() const { return w;}
00142
00143
00144     IMETHOD RotationVel& operator = (const RotationVel& arg);
00145     IMETHOD RotationVel& operator = (const Rotation& arg);
00146         IMETHOD VectorVel UnitX() const;
00147         IMETHOD VectorVel UnitY() const;
00148         IMETHOD VectorVel UnitZ() const;
00149     IMETHOD static RotationVel Identity();
00150     IMETHOD RotationVel Inverse() const;
00151     IMETHOD VectorVel Inverse(const VectorVel& arg) const;
00152     IMETHOD VectorVel Inverse(const Vector& arg) const;
00153     IMETHOD VectorVel operator*(const VectorVel& arg) const;
00154     IMETHOD VectorVel operator*(const Vector& arg) const;
00155     IMETHOD void DoRotX(const doubleVel& angle);
00156     IMETHOD void DoRotY(const doubleVel& angle);
00157     IMETHOD void DoRotZ(const doubleVel& angle);
00158     IMETHOD static RotationVel RotX(const doubleVel& angle);
00159     IMETHOD static RotationVel RotY(const doubleVel& angle);
00160     IMETHOD static RotationVel RotZ(const doubleVel& angle);
00161     IMETHOD static RotationVel Rot(const Vector& rotvec,const doubleVel& angle);
00162     // rotvec has arbitrary norm
00163     // rotation around a constant vector !
00164     IMETHOD static RotationVel Rot2(const Vector& rotvec,const doubleVel& angle);
00165     // rotvec is normalized.
00166     // rotation around a constant vector !
00167     IMETHOD friend RotationVel operator* (const RotationVel& r1,const RotationVel& r2);
00168     IMETHOD friend RotationVel operator* (const Rotation& r1,const RotationVel& r2);
00169     IMETHOD friend RotationVel operator* (const RotationVel& r1,const Rotation& r2);
00170     IMETHOD friend bool Equal(const RotationVel& r1,const RotationVel& r2,double eps=epsilon);
00171     IMETHOD friend bool Equal(const Rotation& r1,const RotationVel& r2,double eps=epsilon);
00172     IMETHOD friend bool Equal(const RotationVel& r1,const Rotation& r2,double eps=epsilon);
00173
00174     IMETHOD TwistVel Inverse(const TwistVel& arg) const;
00175     IMETHOD TwistVel Inverse(const Twist& arg) const;
00176     IMETHOD TwistVel operator * (const TwistVel& arg) const;
00177     IMETHOD TwistVel operator * (const Twist& arg) const;
00178 };
00179
00180
00181
00182
00183 class FrameVel
00184 // = TITLE
00185 //     An FrameVel is a Frame and its first derivative, a Twist vector
00186 // = CLASS TYPE
00187 //     Concrete
00188 // = CAVEATS
00189 //
00190 {
00191 public:
00192     RotationVel M;
00193     VectorVel   p;
00194 public:
00195     FrameVel(){}
00196
00197     explicit FrameVel(const Frame& _T):
00198         M(_T.M),p(_T.p) {}
00199
00200     FrameVel(const Frame& _T,const Twist& _t):
00201         M(_T.M,_t.rot),p(_T.p,_t.vel) {}
00202
00203     FrameVel(const RotationVel& _M,const VectorVel& _p):
00204         M(_M),p(_p) {}
00205
00206
00207     Frame value() const { return Frame(M.value(),p.value());}
00208     Twist deriv() const { return Twist(p.deriv(),M.deriv());}
00209
00210
00211     IMETHOD FrameVel& operator = (const Frame& arg);
00212     IMETHOD FrameVel& operator = (const FrameVel& arg);
00213     IMETHOD static FrameVel Identity();
00214     IMETHOD FrameVel Inverse() const;
00215     IMETHOD VectorVel Inverse(const VectorVel& arg) const;
00216     IMETHOD VectorVel operator*(const VectorVel& arg) const;
00217     IMETHOD VectorVel operator*(const Vector& arg) const;
00218     IMETHOD VectorVel Inverse(const Vector& arg) const;
00219     IMETHOD Frame GetFrame() const;
00220     IMETHOD Twist GetTwist() const;
00221     IMETHOD friend FrameVel operator * (const FrameVel& f1,const FrameVel& f2);
00222     IMETHOD friend FrameVel operator * (const Frame& f1,const FrameVel& f2);
00223     IMETHOD friend FrameVel operator * (const FrameVel& f1,const Frame& f2);
00224     IMETHOD friend bool Equal(const FrameVel& r1,const FrameVel& r2,double eps=epsilon);
00225     IMETHOD friend bool Equal(const Frame& r1,const FrameVel& r2,double eps=epsilon);
00226     IMETHOD friend bool Equal(const FrameVel& r1,const Frame& r2,double eps=epsilon);
00227
00228     IMETHOD TwistVel  Inverse(const TwistVel& arg) const;
00229     IMETHOD TwistVel  Inverse(const Twist& arg) const;
00230     IMETHOD TwistVel operator * (const TwistVel& arg) const;
00231     IMETHOD TwistVel operator * (const Twist& arg) const;
00232 };
00233
00234
00235
00236
00237
00238 //very similar to Wrench class.
00239 class TwistVel
00240 // = TITLE
00241 // This class represents a TwistVel. This is a velocity and rotational velocity together
00242 {
00243 public:
00244     VectorVel vel;
00245     VectorVel rot;
00246 public:
00247
00248 // = Constructors
00249     TwistVel():vel(),rot() {};
00250     TwistVel(const VectorVel& _vel,const VectorVel& _rot):vel(_vel),rot(_rot) {};
00251     TwistVel(const Twist& p,const Twist& v):vel(p.vel, v.vel), rot( p.rot, v.rot) {};
00252     TwistVel(const Twist& p):vel(p.vel), rot( p.rot) {};
00253
00254     Twist value() const {
00255         return Twist(vel.value(),rot.value());
00256     }
00257     Twist deriv() const {
00258         return Twist(vel.deriv(),rot.deriv());
00259     }
00260 // = Operators
00261      IMETHOD TwistVel& operator-=(const TwistVel& arg);
00262      IMETHOD TwistVel& operator+=(const TwistVel& arg);
00263
00264 // = External operators
00265      IMETHOD friend TwistVel operator*(const TwistVel& lhs,double rhs);
00266      IMETHOD friend TwistVel operator*(double lhs,const TwistVel& rhs);
00267      IMETHOD friend TwistVel operator/(const TwistVel& lhs,double rhs);
00268
00269      IMETHOD friend TwistVel operator*(const TwistVel& lhs,const doubleVel& rhs);
00270      IMETHOD friend TwistVel operator*(const doubleVel& lhs,const TwistVel& rhs);
00271      IMETHOD friend TwistVel operator/(const TwistVel& lhs,const doubleVel& rhs);
00272
00273      IMETHOD friend TwistVel operator+(const TwistVel& lhs,const TwistVel& rhs);
00274      IMETHOD friend TwistVel operator-(const TwistVel& lhs,const TwistVel& rhs);
00275      IMETHOD friend TwistVel operator-(const TwistVel& arg);
00276      IMETHOD friend void SetToZero(TwistVel& v);
00277
00278
00279 // = Zero
00280      static IMETHOD TwistVel Zero();
00281
00282 // = Reverse Sign
00283      IMETHOD void ReverseSign();
00284
00285 // = Change Reference point
00286      IMETHOD TwistVel RefPoint(const VectorVel& v_base_AB);
00287      // Changes the reference point of the TwistVel.
00288      // The VectorVel v_base_AB is expressed in the same base as the TwistVel
00289      // The VectorVel v_base_AB is a VectorVel from the old point to
00290      // the new point.
00291      // Complexity : 6M+6A
00292
00293      // = Equality operators
00294      // do not use operator == because the definition of Equal(.,.) is slightly
00295      // different.  It compares whether the 2 arguments are equal in an eps-interval
00296      IMETHOD friend bool Equal(const TwistVel& a,const TwistVel& b,double eps=epsilon);
00297      IMETHOD friend bool Equal(const Twist& a,const TwistVel& b,double eps=epsilon);
00298      IMETHOD friend bool Equal(const TwistVel& a,const Twist& b,double eps=epsilon);
00299
00300 // = Conversion to other entities
00301      IMETHOD Twist GetTwist() const;
00302      IMETHOD Twist GetTwistDot() const;
00303 // = Friends
00304     friend class RotationVel;
00305     friend class FrameVel;
00306
00307 };
00308
00309 IMETHOD VectorVel diff(const VectorVel& a,const VectorVel& b,double dt=1.0) {
00310         return VectorVel(diff(a.p,b.p,dt),diff(a.v,b.v,dt));
00311 }
00312
00313 IMETHOD VectorVel addDelta(const VectorVel& a,const VectorVel&da,double dt=1.0) {
00314         return VectorVel(addDelta(a.p,da.p,dt),addDelta(a.v,da.v,dt));
00315 }
00316 IMETHOD VectorVel diff(const RotationVel& a,const RotationVel& b,double dt = 1.0) {
00317         return VectorVel(diff(a.R,b.R,dt),diff(a.w,b.w,dt));
00318 }
00319
00320 IMETHOD RotationVel addDelta(const RotationVel& a,const VectorVel&da,double dt=1.0) {
00321         return RotationVel(addDelta(a.R,da.p,dt),addDelta(a.w,da.v,dt));
00322 }
00323
00324 IMETHOD TwistVel diff(const FrameVel& a,const FrameVel& b,double dt=1.0) {
00325         return TwistVel(diff(a.M,b.M,dt),diff(a.p,b.p,dt));
00326 }
00327
00328 IMETHOD FrameVel addDelta(const FrameVel& a,const TwistVel& da,double dt=1.0) {
00329         return FrameVel(
00330                         addDelta(a.M,da.rot,dt),
00331                         addDelta(a.p,da.vel,dt)
00332                    );
00333 }
00334
00335 IMETHOD void random(VectorVel& a) {
00336         random(a.p);
00337         random(a.v);
00338 }
00339 IMETHOD void random(TwistVel& a) {
00340         random(a.vel);
00341         random(a.rot);
00342 }
00343
00344 IMETHOD void random(RotationVel& R) {
00345         random(R.R);
00346         random(R.w);
00347 }
00348
00349 IMETHOD void random(FrameVel& F) {
00350         random(F.M);
00351         random(F.p);
00352 }
00353 IMETHOD void posrandom(VectorVel& a) {
00354         posrandom(a.p);
00355         posrandom(a.v);
00356 }
00357 IMETHOD void posrandom(TwistVel& a) {
00358         posrandom(a.vel);
00359         posrandom(a.rot);
00360 }
00361
00362 IMETHOD void posrandom(RotationVel& R) {
00363         posrandom(R.R);
00364         posrandom(R.w);
00365 }
00366
00367 IMETHOD void posrandom(FrameVel& F) {
00368         posrandom(F.M);
00369         posrandom(F.p);
00370 }
00371
00372 #ifdef KDL_INLINE
00373 #include "framevel.inl"
00374 #endif
00375
00376 } // namespace
00377
00378 #endif
00379
00380
00381
00382
```

orocos_kdl
Author(s): Ruben Smits, Erwin Aertbelien, Orocos Developers
autogenerated on Sat Dec 28 2013 17:17:25