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