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 useful 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 // 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) {
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 } // namespace
00391 
00392 #endif
00393 
00394 
00395 
00396 


orocos_kdl
Author(s):
autogenerated on Fri Jun 14 2019 19:33:22