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


orocos_kdl
Author(s):
autogenerated on Wed Aug 26 2015 15:14:14