Blender V2.61 - r43446

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  *      $Name:  $
00020  ****************************************************************************/
00021 
00022 #ifndef KDL_FRAMEVEL_H
00023 #define KDL_FRAMEVEL_H
00024 
00025 #include "utilities/utility.h"
00026 #include "utilities/rall1d.h"
00027 #include "utilities/traits.h"
00028 
00029 #include "frames.hpp"
00030 
00031 
00032 
00033 namespace KDL {
00034 
00035 typedef Rall1d<double> doubleVel;
00036 
00037 IMETHOD doubleVel diff(const doubleVel& a,const doubleVel& b,double dt=1.0) {
00038     return doubleVel((b.t-a.t)/dt,(b.grad-a.grad)/dt);
00039 }
00040 
00041 IMETHOD doubleVel addDelta(const doubleVel& a,const doubleVel&da,double dt=1.0) {
00042     return doubleVel(a.t+da.t*dt,a.grad+da.grad*dt);
00043 }
00044 
00045 IMETHOD void random(doubleVel& F) {
00046     random(F.t);
00047     random(F.grad);
00048 }
00049 IMETHOD void posrandom(doubleVel& F) {
00050     posrandom(F.t);
00051     posrandom(F.grad);
00052 }
00053 
00054 }
00055 
00056 template <>
00057 struct Traits<KDL::doubleVel> {
00058     typedef double valueType;
00059     typedef KDL::doubleVel derivType;
00060 };
00061 
00062 namespace KDL {
00063 
00064 class TwistVel;
00065 class VectorVel;
00066 class FrameVel;
00067 class RotationVel;
00068 
00069 class VectorVel
00070 // = TITLE
00071 //     An VectorVel is a Vector and its first derivative
00072 // = CLASS TYPE
00073 //     Concrete
00074 {
00075 public:
00076     Vector p;       // position vector
00077     Vector v;       // velocity vector
00078 public:
00079     VectorVel():p(),v(){}
00080     VectorVel(const Vector& _p,const Vector& _v):p(_p),v(_v) {}
00081     explicit VectorVel(const Vector& _p):p(_p),v(Vector::Zero()) {}
00082 
00083     Vector value() const { return p;}
00084     Vector deriv() const { return v;}
00085 
00086     IMETHOD VectorVel& operator = (const VectorVel& arg);
00087     IMETHOD VectorVel& operator = (const Vector& arg);
00088     IMETHOD VectorVel& operator += (const VectorVel& arg);
00089     IMETHOD VectorVel& operator -= (const VectorVel& arg);
00090     IMETHOD static VectorVel Zero();
00091     IMETHOD void ReverseSign();
00092     IMETHOD doubleVel Norm() const;
00093     IMETHOD friend VectorVel operator + (const VectorVel& r1,const VectorVel& r2);
00094     IMETHOD friend VectorVel operator - (const VectorVel& r1,const VectorVel& r2);
00095     IMETHOD friend VectorVel operator + (const Vector& r1,const VectorVel& r2);
00096     IMETHOD friend VectorVel operator - (const Vector& r1,const VectorVel& r2);
00097     IMETHOD friend VectorVel operator + (const VectorVel& r1,const Vector& r2);
00098     IMETHOD friend VectorVel operator - (const VectorVel& r1,const Vector& r2);
00099     IMETHOD friend VectorVel operator * (const VectorVel& r1,const VectorVel& r2);
00100     IMETHOD friend VectorVel operator * (const VectorVel& r1,const Vector& r2);
00101     IMETHOD friend VectorVel operator * (const Vector& r1,const VectorVel& r2);
00102     IMETHOD friend VectorVel operator * (const VectorVel& r1,double r2);
00103     IMETHOD friend VectorVel operator * (double r1,const VectorVel& r2);
00104     IMETHOD friend VectorVel operator * (const doubleVel& r1,const VectorVel& r2);
00105     IMETHOD friend VectorVel operator * (const VectorVel& r2,const doubleVel& r1);
00106     IMETHOD friend VectorVel operator*(const Rotation& R,const VectorVel& x);
00107 
00108     IMETHOD friend VectorVel operator / (const VectorVel& r1,double r2);
00109     IMETHOD friend VectorVel operator / (const VectorVel& r2,const doubleVel& r1);
00110     IMETHOD friend void SetToZero(VectorVel& v);
00111 
00112 
00113     IMETHOD friend bool Equal(const VectorVel& r1,const VectorVel& r2,double eps=epsilon);
00114     IMETHOD friend bool Equal(const Vector& r1,const VectorVel& r2,double eps=epsilon);
00115     IMETHOD friend bool Equal(const VectorVel& r1,const Vector& r2,double eps=epsilon);
00116     IMETHOD friend VectorVel operator - (const VectorVel& r);
00117     IMETHOD friend doubleVel dot(const VectorVel& lhs,const VectorVel& rhs);
00118     IMETHOD friend doubleVel dot(const VectorVel& lhs,const Vector& rhs);
00119     IMETHOD friend doubleVel dot(const Vector& lhs,const VectorVel& rhs);
00120 };
00121 
00122 
00123 
00124 class RotationVel
00125 // = TITLE
00126 //     An RotationVel is a Rotation and its first derivative, a rotation vector
00127 // = CLASS TYPE
00128 //     Concrete
00129 {
00130 public:
00131     Rotation R; // Rotation matrix
00132     Vector   w; // rotation vector
00133 public:
00134     RotationVel():R(),w() {}
00135     explicit RotationVel(const Rotation& R_):R(R_),w(Vector::Zero()){}
00136     RotationVel(const Rotation& R_,const Vector& _w):R(R_),w(_w){}
00137 
00138 
00139     Rotation value() const { return R;}
00140     Vector   deriv() const { return w;}
00141 
00142 
00143     IMETHOD RotationVel& operator = (const RotationVel& arg);
00144     IMETHOD RotationVel& operator = (const Rotation& arg);
00145     IMETHOD VectorVel UnitX() const;
00146     IMETHOD VectorVel UnitY() const;
00147     IMETHOD VectorVel UnitZ() const;
00148     IMETHOD static RotationVel Identity();
00149     IMETHOD RotationVel Inverse() const;
00150     IMETHOD VectorVel Inverse(const VectorVel& arg) const;
00151     IMETHOD VectorVel Inverse(const Vector& arg) const;
00152     IMETHOD VectorVel operator*(const VectorVel& arg) const;
00153     IMETHOD VectorVel operator*(const Vector& arg) const;
00154     IMETHOD void DoRotX(const doubleVel& angle);
00155     IMETHOD void DoRotY(const doubleVel& angle);
00156     IMETHOD void DoRotZ(const doubleVel& angle);
00157     IMETHOD static RotationVel RotX(const doubleVel& angle);
00158     IMETHOD static RotationVel RotY(const doubleVel& angle);
00159     IMETHOD static RotationVel RotZ(const doubleVel& angle);
00160     IMETHOD static RotationVel Rot(const Vector& rotvec,const doubleVel& angle);
00161     // rotvec has arbitrary norm
00162     // rotation around a constant vector !
00163     IMETHOD static RotationVel Rot2(const Vector& rotvec,const doubleVel& angle);
00164     // rotvec is normalized.
00165     // rotation around a constant vector !
00166     IMETHOD friend RotationVel operator* (const RotationVel& r1,const RotationVel& r2);
00167     IMETHOD friend RotationVel operator* (const Rotation& r1,const RotationVel& r2);
00168     IMETHOD friend RotationVel operator* (const RotationVel& r1,const Rotation& r2);
00169     IMETHOD friend bool Equal(const RotationVel& r1,const RotationVel& r2,double eps=epsilon);
00170     IMETHOD friend bool Equal(const Rotation& r1,const RotationVel& r2,double eps=epsilon);
00171     IMETHOD friend bool Equal(const RotationVel& r1,const Rotation& r2,double eps=epsilon);
00172 
00173     IMETHOD TwistVel Inverse(const TwistVel& arg) const;
00174     IMETHOD TwistVel Inverse(const Twist& arg) const;
00175     IMETHOD TwistVel operator * (const TwistVel& arg) const;
00176     IMETHOD TwistVel operator * (const Twist& arg) const;
00177 };
00178 
00179 
00180 
00181 
00182 class FrameVel
00183 // = TITLE
00184 //     An FrameVel is a Frame and its first derivative, a Twist vector
00185 // = CLASS TYPE
00186 //     Concrete
00187 // = CAVEATS
00188 //
00189 {
00190 public:
00191     RotationVel M;
00192     VectorVel   p;
00193 public:
00194     FrameVel(){}
00195 
00196     explicit FrameVel(const Frame& T_):
00197         M(T_.M),p(T_.p) {}
00198 
00199     FrameVel(const Frame& T_,const Twist& _t):
00200         M(T_.M,_t.rot),p(T_.p,_t.vel) {}
00201 
00202     FrameVel(const RotationVel& _M,const VectorVel& _p):
00203         M(_M),p(_p) {}
00204 
00205 
00206     Frame value() const { return Frame(M.value(),p.value());}
00207     Twist deriv() const { return Twist(p.deriv(),M.deriv());}
00208 
00209 
00210     IMETHOD FrameVel& operator = (const Frame& arg);
00211     IMETHOD FrameVel& operator = (const FrameVel& arg);
00212     IMETHOD static FrameVel Identity();
00213     IMETHOD FrameVel Inverse() const;
00214     IMETHOD VectorVel Inverse(const VectorVel& arg) const;
00215     IMETHOD VectorVel operator*(const VectorVel& arg) const;
00216     IMETHOD VectorVel operator*(const Vector& arg) const;
00217     IMETHOD VectorVel Inverse(const Vector& arg) const;
00218     IMETHOD Frame GetFrame() const;
00219     IMETHOD Twist GetTwist() const;
00220     IMETHOD friend FrameVel operator * (const FrameVel& f1,const FrameVel& f2);
00221     IMETHOD friend FrameVel operator * (const Frame& f1,const FrameVel& f2);
00222     IMETHOD friend FrameVel operator * (const FrameVel& f1,const Frame& f2);
00223     IMETHOD friend bool Equal(const FrameVel& r1,const FrameVel& r2,double eps=epsilon);
00224     IMETHOD friend bool Equal(const Frame& r1,const FrameVel& r2,double eps=epsilon);
00225     IMETHOD friend bool Equal(const FrameVel& r1,const Frame& r2,double eps=epsilon);
00226 
00227     IMETHOD TwistVel  Inverse(const TwistVel& arg) const;
00228     IMETHOD TwistVel  Inverse(const Twist& arg) const;
00229     IMETHOD TwistVel operator * (const TwistVel& arg) const;
00230     IMETHOD TwistVel operator * (const Twist& arg) const;
00231 };
00232 
00233 
00234 
00235 
00236 
00237 //very similar to Wrench class.
00238 class TwistVel
00239 // = TITLE
00240 // This class represents a TwistVel. This is a velocity and rotational velocity together
00241 {
00242 public:
00243     VectorVel vel;
00244     VectorVel rot;
00245 public:
00246 
00247 // = Constructors
00248     TwistVel():vel(),rot() {};
00249     TwistVel(const VectorVel& _vel,const VectorVel& _rot):vel(_vel),rot(_rot) {};
00250     TwistVel(const Twist& p,const Twist& v):vel(p.vel, v.vel), rot( p.rot, v.rot) {};
00251     TwistVel(const Twist& p):vel(p.vel), rot( p.rot) {};
00252 
00253     Twist value() const {
00254         return Twist(vel.value(),rot.value());
00255     }
00256     Twist deriv() const {
00257         return Twist(vel.deriv(),rot.deriv());
00258     }
00259 // = Operators
00260      IMETHOD TwistVel& operator-=(const TwistVel& arg);
00261      IMETHOD TwistVel& operator+=(const TwistVel& arg);
00262 
00263 // = External operators
00264      IMETHOD friend TwistVel operator*(const TwistVel& lhs,double rhs);
00265      IMETHOD friend TwistVel operator*(double lhs,const TwistVel& rhs);
00266      IMETHOD friend TwistVel operator/(const TwistVel& lhs,double rhs);
00267 
00268      IMETHOD friend TwistVel operator*(const TwistVel& lhs,const doubleVel& rhs);
00269      IMETHOD friend TwistVel operator*(const doubleVel& lhs,const TwistVel& rhs);
00270      IMETHOD friend TwistVel operator/(const TwistVel& lhs,const doubleVel& rhs);
00271 
00272      IMETHOD friend TwistVel operator+(const TwistVel& lhs,const TwistVel& rhs);
00273      IMETHOD friend TwistVel operator-(const TwistVel& lhs,const TwistVel& rhs);
00274      IMETHOD friend TwistVel operator-(const TwistVel& arg);
00275      IMETHOD friend void SetToZero(TwistVel& v);
00276 
00277 
00278 // = Zero
00279      static IMETHOD TwistVel Zero();
00280 
00281 // = Reverse Sign
00282      IMETHOD void ReverseSign();
00283 
00284 // = Change Reference point
00285      IMETHOD TwistVel RefPoint(const VectorVel& v_base_AB);
00286      // Changes the reference point of the TwistVel.
00287      // The VectorVel v_base_AB is expressed in the same base as the TwistVel
00288      // The VectorVel v_base_AB is a VectorVel from the old point to
00289      // the new point.
00290      // Complexity : 6M+6A
00291 
00292      // = Equality operators
00293      // do not use operator == because the definition of Equal(.,.) is slightly
00294      // different.  It compares whether the 2 arguments are equal in an eps-interval
00295      IMETHOD friend bool Equal(const TwistVel& a,const TwistVel& b,double eps=epsilon);
00296      IMETHOD friend bool Equal(const Twist& a,const TwistVel& b,double eps=epsilon);
00297      IMETHOD friend bool Equal(const TwistVel& a,const Twist& b,double eps=epsilon);
00298 
00299 // = Conversion to other entities
00300      IMETHOD Twist GetTwist() const;
00301      IMETHOD Twist GetTwistDot() const;
00302 // = Friends
00303     friend class RotationVel;
00304     friend class FrameVel;
00305 
00306 };
00307 
00308 IMETHOD VectorVel diff(const VectorVel& a,const VectorVel& b,double dt=1.0) {
00309     return VectorVel(diff(a.p,b.p,dt),diff(a.v,b.v,dt));
00310 }
00311 
00312 IMETHOD VectorVel addDelta(const VectorVel& a,const VectorVel&da,double dt=1.0) {
00313     return VectorVel(addDelta(a.p,da.p,dt),addDelta(a.v,da.v,dt));
00314 }
00315 IMETHOD VectorVel diff(const RotationVel& a,const RotationVel& b,double dt = 1.0) {
00316     return VectorVel(diff(a.R,b.R,dt),diff(a.w,b.w,dt));
00317 }
00318 
00319 IMETHOD RotationVel addDelta(const RotationVel& a,const VectorVel&da,double dt=1.0) {
00320     return RotationVel(addDelta(a.R,da.p,dt),addDelta(a.w,da.v,dt));
00321 }
00322 
00323 IMETHOD TwistVel diff(const FrameVel& a,const FrameVel& b,double dt=1.0) {
00324     return TwistVel(diff(a.M,b.M,dt),diff(a.p,b.p,dt));
00325 }
00326 
00327 IMETHOD FrameVel addDelta(const FrameVel& a,const TwistVel& da,double dt=1.0) {
00328     return FrameVel(
00329             addDelta(a.M,da.rot,dt),
00330             addDelta(a.p,da.vel,dt)
00331            );
00332 }
00333 
00334 IMETHOD void random(VectorVel& a) {
00335     random(a.p);
00336     random(a.v);
00337 }
00338 IMETHOD void random(TwistVel& a) {
00339     random(a.vel);
00340     random(a.rot);
00341 }
00342 
00343 IMETHOD void random(RotationVel& R) {
00344     random(R.R);
00345     random(R.w);
00346 }
00347 
00348 IMETHOD void random(FrameVel& F) {
00349     random(F.M);
00350     random(F.p);
00351 }
00352 IMETHOD void posrandom(VectorVel& a) {
00353     posrandom(a.p);
00354     posrandom(a.v);
00355 }
00356 IMETHOD void posrandom(TwistVel& a) {
00357     posrandom(a.vel);
00358     posrandom(a.rot);
00359 }
00360 
00361 IMETHOD void posrandom(RotationVel& R) {
00362     posrandom(R.R);
00363     posrandom(R.w);
00364 }
00365 
00366 IMETHOD void posrandom(FrameVel& F) {
00367     posrandom(F.M);
00368     posrandom(F.p);
00369 }
00370 
00371 #ifdef KDL_INLINE
00372 #include "framevel.inl"
00373 #endif
00374 
00375 } // namespace
00376 
00377 #endif
00378 
00379 
00380 
00381