Blender V2.61 - r43446

frameacc.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 set (value,derivative,2nd derivative)
00006  *      and define operations on that set
00007  *      this classes are usefull for automatic differentiation ( <-> symbolic diff ,
00008  *      <-> numeric diff).
00009  *      Defines VectorAcc, RotationAcc, FrameAcc, doubleAcc.
00010  *      Look at the corresponding classes Vector Rotation Frame Twist and
00011  *      Wrench for the semantics of the methods.
00012  *
00013  *      It also contains the 2nd derivative <-> RFrames.h
00014  *
00015  *  \author
00016  *      Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
00017  *
00018  *  \version
00019  *      ORO_Geometry V0.2
00020  *
00021  *  \par History
00022  *      - $log$
00023  *
00024  *  \par Release
00025  *      $Name:  $
00026  ****************************************************************************/
00027 
00028 #ifndef RRFRAMES_H
00029 #define RRFRAMES_H
00030 
00031 
00032 #include "utilities/rall2d.h"
00033 #include "frames.hpp"
00034 
00035 
00036 
00037 namespace KDL {
00038 
00039 class TwistAcc;
00040 typedef Rall2d<double,double,double> doubleAcc;
00041 
00042 
00043 class VectorAcc
00044 {
00045 public:
00046     Vector p;   
00047     Vector v;   
00048     Vector dv;  
00049 public:
00050     VectorAcc():p(),v(),dv() {}
00051     explicit VectorAcc(const Vector& _p):p(_p),v(Vector::Zero()),dv(Vector::Zero()) {}
00052     VectorAcc(const Vector& _p,const Vector& _v):p(_p),v(_v),dv(Vector::Zero()) {}
00053     VectorAcc(const Vector& _p,const Vector& _v,const Vector& _dv):
00054         p(_p),v(_v),dv(_dv) {}
00055     IMETHOD VectorAcc& operator = (const VectorAcc& arg);
00056     IMETHOD VectorAcc& operator = (const Vector& arg);
00057     IMETHOD VectorAcc& operator += (const VectorAcc& arg);
00058     IMETHOD VectorAcc& operator -= (const VectorAcc& arg);
00059     IMETHOD static VectorAcc Zero();
00060     IMETHOD void ReverseSign();
00061     IMETHOD doubleAcc Norm();
00062     IMETHOD friend VectorAcc operator + (const VectorAcc& r1,const VectorAcc& r2);
00063     IMETHOD friend VectorAcc operator - (const VectorAcc& r1,const VectorAcc& r2);
00064     IMETHOD friend VectorAcc operator + (const Vector& r1,const VectorAcc& r2);
00065     IMETHOD friend VectorAcc operator - (const Vector& r1,const VectorAcc& r2);
00066     IMETHOD friend VectorAcc operator + (const VectorAcc& r1,const Vector& r2);
00067     IMETHOD friend VectorAcc operator - (const VectorAcc& r1,const Vector& r2);
00068     IMETHOD friend VectorAcc operator * (const VectorAcc& r1,const VectorAcc& r2);
00069     IMETHOD friend VectorAcc operator * (const VectorAcc& r1,const Vector& r2);
00070     IMETHOD friend VectorAcc operator * (const Vector& r1,const VectorAcc& r2);
00071     IMETHOD friend VectorAcc operator * (const VectorAcc& r1,double r2);
00072     IMETHOD friend VectorAcc operator * (double r1,const VectorAcc& r2);
00073     IMETHOD friend VectorAcc operator * (const doubleAcc& r1,const VectorAcc& r2);
00074     IMETHOD friend VectorAcc operator * (const VectorAcc& r2,const doubleAcc& r1);
00075     IMETHOD friend VectorAcc operator*(const Rotation& R,const VectorAcc& x);
00076 
00077     IMETHOD friend VectorAcc operator / (const VectorAcc& r1,double r2);
00078     IMETHOD friend VectorAcc operator / (const VectorAcc& r2,const doubleAcc& r1);
00079 
00080 
00081     IMETHOD friend bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps=epsilon);
00082     IMETHOD friend bool Equal(const Vector& r1,const VectorAcc& r2,double eps=epsilon);
00083     IMETHOD friend bool Equal(const VectorAcc& r1,const Vector& r2,double eps=epsilon);
00084     IMETHOD friend VectorAcc operator - (const VectorAcc& r);
00085     IMETHOD friend doubleAcc dot(const VectorAcc& lhs,const VectorAcc& rhs);
00086     IMETHOD friend doubleAcc dot(const VectorAcc& lhs,const Vector& rhs);
00087     IMETHOD friend doubleAcc dot(const Vector& lhs,const VectorAcc& rhs);
00088 };
00089 
00090 
00091 
00092 class RotationAcc
00093 {
00094 public:
00095     Rotation R;     
00096     Vector   w;     
00097     Vector   dw;    
00098 public:
00099     RotationAcc():R(),w() {}
00100     explicit RotationAcc(const Rotation& R_):R(R_),w(Vector::Zero()){}
00101     RotationAcc(const Rotation& R_,const Vector& _w,const Vector& _dw):
00102         R(R_),w(_w),dw(_dw) {}
00103     IMETHOD RotationAcc& operator = (const RotationAcc& arg);
00104     IMETHOD RotationAcc& operator = (const Rotation& arg);
00105     IMETHOD static RotationAcc Identity();
00106     IMETHOD RotationAcc Inverse() const;
00107     IMETHOD VectorAcc Inverse(const VectorAcc& arg) const;
00108     IMETHOD VectorAcc Inverse(const Vector& arg) const;
00109     IMETHOD VectorAcc operator*(const VectorAcc& arg) const;
00110     IMETHOD VectorAcc operator*(const Vector& arg) const;
00111 
00112     //  Rotations
00113     // The SetRot.. functions set the value of *this to the appropriate rotation matrix.
00114     // The Rot... static functions give the value of the appropriate rotation matrix back.
00115     // The DoRot... functions apply a rotation R to *this,such that *this = *this * R.
00116     // IMETHOD void DoRotX(const doubleAcc& angle);
00117     // IMETHOD void DoRotY(const doubleAcc& angle);
00118     // IMETHOD void DoRotZ(const doubleAcc& angle);
00119     // IMETHOD static RRotation RotX(const doubleAcc& angle);
00120     // IMETHOD static RRotation RotY(const doubleAcc& angle);
00121     // IMETHOD static RRotation RotZ(const doubleAcc& angle);
00122 
00123     // IMETHOD void SetRot(const Vector& rotaxis,const doubleAcc& angle);
00124     // Along an arbitrary axes.  The norm of rotvec is neglected.
00125     // IMETHOD static RotationAcc Rot(const Vector& rotvec,const doubleAcc& angle);
00126     // rotvec has arbitrary norm
00127     // rotation around a constant vector !
00128     // IMETHOD static RotationAcc Rot2(const Vector& rotvec,const doubleAcc& angle);
00129     // rotvec is normalized.
00130     // rotation around a constant vector !
00131 
00132     IMETHOD friend RotationAcc operator* (const RotationAcc& r1,const RotationAcc& r2);
00133     IMETHOD friend RotationAcc operator* (const Rotation& r1,const RotationAcc& r2);
00134     IMETHOD friend RotationAcc operator* (const RotationAcc& r1,const Rotation& r2);
00135     IMETHOD friend bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps=epsilon);
00136     IMETHOD friend bool Equal(const Rotation& r1,const RotationAcc& r2,double eps=epsilon);
00137     IMETHOD friend bool Equal(const RotationAcc& r1,const Rotation& r2,double eps=epsilon);
00138     IMETHOD TwistAcc Inverse(const TwistAcc& arg) const;
00139     IMETHOD TwistAcc Inverse(const Twist& arg) const;
00140     IMETHOD TwistAcc operator * (const TwistAcc& arg) const;
00141     IMETHOD TwistAcc operator * (const Twist& arg) const;
00142 };
00143 
00144 
00145 
00146 
00147 class FrameAcc
00148 {
00149 public:
00150     RotationAcc M;   
00151     VectorAcc   p;   
00152 public:
00153     FrameAcc(){}
00154     explicit FrameAcc(const Frame& T_):M(T_.M),p(T_.p) {}
00155     FrameAcc(const Frame& T_,const Twist& _t,const Twist& _dt):
00156         M(T_.M,_t.rot,_dt.rot),p(T_.p,_t.vel,_dt.vel) {}
00157     FrameAcc(const RotationAcc& _M,const VectorAcc& _p):M(_M),p(_p) {}
00158 
00159     IMETHOD FrameAcc& operator = (const FrameAcc& arg);
00160     IMETHOD FrameAcc& operator = (const Frame& arg);
00161     IMETHOD static FrameAcc Identity();
00162     IMETHOD FrameAcc Inverse() const;
00163     IMETHOD VectorAcc Inverse(const VectorAcc& arg) const;
00164     IMETHOD VectorAcc operator*(const VectorAcc& arg) const;
00165     IMETHOD VectorAcc operator*(const Vector& arg) const;
00166     IMETHOD VectorAcc Inverse(const Vector& arg) const;
00167     IMETHOD Frame GetFrame() const;
00168     IMETHOD Twist GetTwist() const;
00169     IMETHOD Twist GetAccTwist() const;
00170     IMETHOD friend FrameAcc operator * (const FrameAcc& f1,const FrameAcc& f2);
00171     IMETHOD friend FrameAcc operator * (const Frame& f1,const FrameAcc& f2);
00172     IMETHOD friend FrameAcc operator * (const FrameAcc& f1,const Frame& f2);
00173     IMETHOD friend bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps=epsilon);
00174     IMETHOD friend bool Equal(const Frame& r1,const FrameAcc& r2,double eps=epsilon);
00175     IMETHOD friend bool Equal(const FrameAcc& r1,const Frame& r2,double eps=epsilon);
00176 
00177     IMETHOD TwistAcc  Inverse(const TwistAcc& arg) const;
00178     IMETHOD TwistAcc  Inverse(const Twist& arg) const;
00179     IMETHOD TwistAcc operator * (const TwistAcc& arg) const;
00180     IMETHOD TwistAcc operator * (const Twist& arg) const;
00181 };
00182 
00183 
00184 
00185 
00186 
00187 
00188 
00189 
00190 //very similar to Wrench class.
00191 class TwistAcc
00192 {
00193 public:
00194     VectorAcc vel;       
00195     VectorAcc rot;       
00196 public:
00197 
00198      TwistAcc():vel(),rot() {};
00199      TwistAcc(const VectorAcc& _vel,const VectorAcc& _rot):vel(_vel),rot(_rot) {};
00200 
00201      IMETHOD TwistAcc& operator-=(const TwistAcc& arg);
00202      IMETHOD TwistAcc& operator+=(const TwistAcc& arg);
00203 
00204      IMETHOD friend TwistAcc operator*(const TwistAcc& lhs,double rhs);
00205      IMETHOD friend TwistAcc operator*(double lhs,const TwistAcc& rhs);
00206      IMETHOD friend TwistAcc operator/(const TwistAcc& lhs,double rhs);
00207 
00208      IMETHOD friend TwistAcc operator*(const TwistAcc& lhs,const doubleAcc& rhs);
00209      IMETHOD friend TwistAcc operator*(const doubleAcc& lhs,const TwistAcc& rhs);
00210      IMETHOD friend TwistAcc operator/(const TwistAcc& lhs,const doubleAcc& rhs);
00211 
00212      IMETHOD friend TwistAcc operator+(const TwistAcc& lhs,const TwistAcc& rhs);
00213      IMETHOD friend TwistAcc operator-(const TwistAcc& lhs,const TwistAcc& rhs);
00214      IMETHOD friend TwistAcc operator-(const TwistAcc& arg);
00215 
00216      IMETHOD friend void SetToZero(TwistAcc& v);
00217 
00218      static IMETHOD TwistAcc Zero();
00219 
00220      IMETHOD void ReverseSign();
00221 
00222      IMETHOD TwistAcc RefPoint(const VectorAcc& v_base_AB);
00223      // Changes the reference point of the RTwist.
00224      // The RVector v_base_AB is expressed in the same base as the RTwist
00225      // The RVector v_base_AB is a RVector from the old point to
00226      // the new point.
00227      // Complexity : 6M+6A
00228 
00229      IMETHOD friend bool Equal(const TwistAcc& a,const TwistAcc& b,double eps=epsilon);
00230      IMETHOD friend bool Equal(const Twist& a,const TwistAcc& b,double eps=epsilon);
00231      IMETHOD friend bool Equal(const TwistAcc& a,const Twist& b,double eps=epsilon);
00232 
00233 
00234      IMETHOD Twist GetTwist() const;
00235      IMETHOD Twist GetTwistDot() const;
00236 
00237     friend class RotationAcc;
00238     friend class FrameAcc;
00239 
00240 };
00241 
00242 
00243 
00244 
00245 
00246 
00247 
00248 #ifdef KDL_INLINE
00249 #include "frameacc.inl"
00250 #endif
00251 
00252 }
00253 
00254 
00255 
00256 
00257 
00258 #endif