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) {
00040 }
00041
00042 IMETHOD doubleVel addDelta(const doubleVel& a,const doubleVel&da,double dt=1.0) {
00044 }
00045
00046 IMETHOD void random(doubleVel& F) {
00047         random(F.t);
00049 }
00050 IMETHOD void posrandom(doubleVel& F) {
00051         posrandom(F.t);
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 // Equal is friend function, but default arguments for friends are forbidden (ยง8.3.6.4)
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 // = TITLE
00086 //     An VectorVel is a Vector and its first derivative
00087 // = CLASS TYPE
00088 //     Concrete
00089 {
00090 public:
00091     Vector p;       // position vector
00092     Vector v;       // velocity vector
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 // = TITLE
00141 //     An RotationVel is a Rotation and its first derivative, a rotation vector
00142 // = CLASS TYPE
00143 //     Concrete
00144 {
00145 public:
00146     Rotation R; // Rotation matrix
00147     Vector   w; // rotation vector
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     // rotvec has arbitrary norm
00177     // rotation around a constant vector !
00178     IMETHOD static RotationVel Rot2(const Vector& rotvec,const doubleVel& angle);
00179     // rotvec is normalized.
00180     // rotation around a constant vector !
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 // = TITLE
00199 //     An FrameVel is a Frame and its first derivative, a Twist vector
00200 // = CLASS TYPE
00201 //     Concrete
00202 // = CAVEATS
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 //very similar to Wrench class.
00253 class TwistVel
00254 // = TITLE
00255 // This class represents a TwistVel. This is a velocity and rotational velocity together
00256 {
00257 public:
00258     VectorVel vel;
00259     VectorVel rot;
00260 public:
00261
00262 // = Constructors
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 // = Operators
00275      IMETHOD TwistVel& operator-=(const TwistVel& arg);
00276      IMETHOD TwistVel& operator+=(const TwistVel& arg);
00277
00278 // = External operators
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 // = Zero
00294      static IMETHOD TwistVel Zero();
00295
00296 // = Reverse Sign
00297      IMETHOD void ReverseSign();
00298
00299 // = Change Reference point
00300      IMETHOD TwistVel RefPoint(const VectorVel& v_base_AB);
00301      // Changes the reference point of the TwistVel.
00302      // The VectorVel v_base_AB is expressed in the same base as the TwistVel
00303      // The VectorVel v_base_AB is a VectorVel from the old point to
00304      // the new point.
00305      // Complexity : 6M+6A
00306
00307      // = Equality operators
00308      // do not use operator == because the definition of Equal(.,.) is slightly
00309      // different.  It compares whether the 2 arguments are equal in an eps-interval
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 // = Conversion to other entities
00315      IMETHOD Twist GetTwist() const;
00316      IMETHOD Twist GetTwistDot() const;
00317 // = Friends
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) {
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) {
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(
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 } // namespace
00391
00392 #endif
00393
00394
00395
00396
```

orocos_kdl
Author(s):
autogenerated on Sat Oct 7 2017 03:04:28