00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00124 #ifndef KDL_FRAMES_H
00125 #define KDL_FRAMES_H
00126
00127
00128 #include "utilities/kdl-config.h"
00129 #include "utilities/utility.h"
00130
00132
00133 namespace KDL {
00134
00135
00136
00137 class Vector;
00138 class Rotation;
00139 class Frame;
00140 class Wrench;
00141 class Twist;
00142 class Vector2;
00143 class Rotation2;
00144 class Frame2;
00145
00146
00147
00148 inline bool Equal(const Vector& a,const Vector& b,double eps=epsilon);
00149 inline bool Equal(const Frame& a,const Frame& b,double eps=epsilon);
00150 inline bool Equal(const Twist& a,const Twist& b,double eps=epsilon);
00151 inline bool Equal(const Wrench& a,const Wrench& b,double eps=epsilon);
00152 inline bool Equal(const Vector2& a,const Vector2& b,double eps=epsilon);
00153 inline bool Equal(const Rotation2& a,const Rotation2& b,double eps=epsilon);
00154 inline bool Equal(const Frame2& a,const Frame2& b,double eps=epsilon);
00155
00156
00160 class Vector
00161 {
00162 public:
00163 double data[3];
00165 inline Vector() {data[0]=data[1]=data[2] = 0.0;}
00166
00168 inline Vector(double x,double y, double z);
00169
00171 inline Vector(const Vector& arg);
00172
00174 inline Vector& operator = ( const Vector& arg);
00175
00177 inline double operator()(int index) const;
00178
00180 inline double& operator() (int index);
00181
00183 double operator[] ( int index ) const
00184 {
00185 return this->operator() ( index );
00186 }
00187
00189 double& operator[] ( int index )
00190 {
00191 return this->operator() ( index );
00192 }
00193
00194 inline double x() const;
00195 inline double y() const;
00196 inline double z() const;
00197 inline void x(double);
00198 inline void y(double);
00199 inline void z(double);
00200
00202 inline void ReverseSign();
00203
00204
00206 inline Vector& operator-=(const Vector& arg);
00207
00208
00210 inline Vector& operator +=(const Vector& arg);
00211
00213 inline friend Vector operator*(const Vector& lhs,double rhs);
00215 inline friend Vector operator*(double lhs,const Vector& rhs);
00217
00218 inline friend Vector operator/(const Vector& lhs,double rhs);
00219 inline friend Vector operator+(const Vector& lhs,const Vector& rhs);
00220 inline friend Vector operator-(const Vector& lhs,const Vector& rhs);
00221 inline friend Vector operator*(const Vector& lhs,const Vector& rhs);
00222 inline friend Vector operator-(const Vector& arg);
00223 inline friend double dot(const Vector& lhs,const Vector& rhs);
00224
00227 inline friend void SetToZero(Vector& v);
00228
00230 inline static Vector Zero();
00231
00237 double Normalize(double eps=epsilon);
00238
00240 double Norm() const;
00241
00242
00243
00245 inline void Set2DXY(const Vector2& v);
00247 inline void Set2DYZ(const Vector2& v);
00249 inline void Set2DZX(const Vector2& v);
00251 inline void Set2DPlane(const Frame& F_someframe_XY,const Vector2& v_XY);
00252
00253
00256 inline friend bool Equal(const Vector& a,const Vector& b,double eps);
00257
00259 inline friend bool operator==(const Vector& a,const Vector& b);
00261 inline friend bool operator!=(const Vector& a,const Vector& b);
00262
00263 friend class Rotation;
00264 friend class Frame;
00265 };
00266
00267
00301 class Rotation
00302 {
00303 public:
00304 double data[9];
00305
00306 inline Rotation() {
00307 *this = Rotation::Identity();
00308 }
00309 inline Rotation(double Xx,double Yx,double Zx,
00310 double Xy,double Yy,double Zy,
00311 double Xz,double Yz,double Zz);
00312 inline Rotation(const Vector& x,const Vector& y,const Vector& z);
00313
00314
00315
00316 inline Rotation& operator=(const Rotation& arg);
00317
00320 inline Vector operator*(const Vector& v) const;
00321
00323 inline double& operator()(int i,int j);
00324
00326 inline double operator() (int i,int j) const;
00327
00328 friend Rotation operator *(const Rotation& lhs,const Rotation& rhs);
00329
00331 inline void SetInverse();
00332
00334 inline Rotation Inverse() const;
00335
00337 inline Vector Inverse(const Vector& v) const;
00338
00340 inline Wrench Inverse(const Wrench& arg) const;
00341
00343 inline Twist Inverse(const Twist& arg) const;
00344
00346 inline static Rotation Identity();
00347
00348
00349
00351 inline static Rotation RotX(double angle);
00353 inline static Rotation RotY(double angle);
00355 inline static Rotation RotZ(double angle);
00358 inline void DoRotX(double angle);
00361 inline void DoRotY(double angle);
00364 inline void DoRotZ(double angle);
00365
00369
00370 static Rotation Rot(const Vector& rotvec,double angle);
00371
00373 static Rotation Rot2(const Vector& rotvec,double angle);
00374
00377 Vector GetRot() const;
00378
00387 double GetRotAngle(Vector& axis,double eps=epsilon) const;
00388
00389
00398 static Rotation EulerZYZ(double Alfa,double Beta,double Gamma);
00399
00415 void GetEulerZYZ(double& alpha,double& beta,double& gamma) const;
00416
00419 static Rotation Quaternion(double x,double y,double z, double w);
00420
00423 void GetQuaternion(double& x,double& y,double& z, double& w) const;
00424
00435 static Rotation RPY(double roll,double pitch,double yaw);
00436
00455 void GetRPY(double& roll,double& pitch,double& yaw) const;
00456
00457
00469 inline static Rotation EulerZYX(double Alfa,double Beta,double Gamma) {
00470 return RPY(Gamma,Beta,Alfa);
00471 }
00472
00493 inline void GetEulerZYX(double& Alfa,double& Beta,double& Gamma) const {
00494 GetRPY(Gamma,Beta,Alfa);
00495 }
00496
00501 inline Twist operator * (const Twist& arg) const;
00502
00507 inline Wrench operator * (const Wrench& arg) const;
00508
00510 inline Vector UnitX() const {
00511 return Vector(data[0],data[3],data[6]);
00512 }
00513
00515 inline void UnitX(const Vector& X) {
00516 data[0] = X(0);
00517 data[3] = X(1);
00518 data[6] = X(2);
00519 }
00520
00522 inline Vector UnitY() const {
00523 return Vector(data[1],data[4],data[7]);
00524 }
00525
00527 inline void UnitY(const Vector& X) {
00528 data[1] = X(0);
00529 data[4] = X(1);
00530 data[7] = X(2);
00531 }
00532
00534 inline Vector UnitZ() const {
00535 return Vector(data[2],data[5],data[8]);
00536 }
00537
00539 inline void UnitZ(const Vector& X) {
00540 data[2] = X(0);
00541 data[5] = X(1);
00542 data[8] = X(2);
00543 }
00544
00547 friend bool Equal(const Rotation& a,const Rotation& b,double eps);
00548
00550 friend bool operator==(const Rotation& a,const Rotation& b);
00552 friend bool operator!=(const Rotation& a,const Rotation& b);
00553
00554 friend class Frame;
00555 };
00556 bool operator==(const Rotation& a,const Rotation& b);
00557 bool Equal(const Rotation& a,const Rotation& b,double eps=epsilon);
00558
00559
00560
00570 class Frame {
00571 public:
00572 Vector p;
00573 Rotation M;
00574
00575 public:
00576
00577 inline Frame(const Rotation& R,const Vector& V);
00578
00580 explicit inline Frame(const Vector& V);
00582 explicit inline Frame(const Rotation& R);
00583
00584 inline Frame() {}
00586 inline Frame(const Frame& arg);
00587
00589
00590 void Make4x4(double* d);
00591
00594 inline double operator()(int i,int j);
00595
00598 inline double operator() (int i,int j) const;
00599
00600
00602 inline Frame Inverse() const;
00603
00605 inline Vector Inverse(const Vector& arg) const;
00606
00608 inline Wrench Inverse(const Wrench& arg) const;
00609
00611 inline Twist Inverse(const Twist& arg) const;
00612
00614 inline Frame& operator = (const Frame& arg);
00615
00618 inline Vector operator * (const Vector& arg) const;
00619
00626 inline Wrench operator * (const Wrench& arg) const;
00627
00634 inline Twist operator * (const Twist& arg) const;
00635
00637 inline friend Frame operator *(const Frame& lhs,const Frame& rhs);
00638
00640 inline static Frame Identity();
00641
00645 inline void Integrate(const Twist& t_this,double frequency);
00646
00647
00648
00649
00650
00651
00652
00653
00654
00655
00656
00657
00658
00659
00660
00661
00662
00663
00664
00665
00666
00667
00668
00669
00670
00671
00672
00673
00674
00675
00676
00677
00678
00679
00680
00681
00682
00683
00684
00685
00686
00687
00688
00689
00690
00691
00692
00693 static Frame DH_Craig1989(double a,double alpha,double d,double theta);
00694
00695
00696
00697
00698
00699
00700
00701 static Frame DH(double a,double alpha,double d,double theta);
00702
00703
00706 inline friend bool Equal(const Frame& a,const Frame& b,double eps);
00707
00709 inline friend bool operator==(const Frame& a,const Frame& b);
00711 inline friend bool operator!=(const Frame& a,const Frame& b);
00712 };
00713
00720 class Twist {
00721 public:
00722 Vector vel;
00723 Vector rot;
00724 public:
00725
00727 Twist():vel(),rot() {};
00728
00729 Twist(const Vector& _vel,const Vector& _rot):vel(_vel),rot(_rot) {};
00730
00731 inline Twist& operator-=(const Twist& arg);
00732 inline Twist& operator+=(const Twist& arg);
00734 inline double& operator()(int i);
00735
00738 inline double operator()(int i) const;
00739
00740 double operator[] ( int index ) const
00741 {
00742 return this->operator() ( index );
00743 }
00744
00745 double& operator[] ( int index )
00746 {
00747 return this->operator() ( index );
00748 }
00749
00750 inline friend Twist operator*(const Twist& lhs,double rhs);
00751 inline friend Twist operator*(double lhs,const Twist& rhs);
00752 inline friend Twist operator/(const Twist& lhs,double rhs);
00753 inline friend Twist operator+(const Twist& lhs,const Twist& rhs);
00754 inline friend Twist operator-(const Twist& lhs,const Twist& rhs);
00755 inline friend Twist operator-(const Twist& arg);
00756 inline friend double dot(const Twist& lhs,const Wrench& rhs);
00757 inline friend double dot(const Wrench& rhs,const Twist& lhs);
00758 inline friend void SetToZero(Twist& v);
00760 inline friend Twist operator*(const Twist& lhs,const Twist& rhs);
00762 inline friend Wrench operator*(const Twist& lhs,const Wrench& rhs);
00763
00765 static inline Twist Zero();
00766
00768 inline void ReverseSign();
00769
00776 inline Twist RefPoint(const Vector& v_base_AB) const;
00777
00778
00781 inline friend bool Equal(const Twist& a,const Twist& b,double eps);
00782
00784 inline friend bool operator==(const Twist& a,const Twist& b);
00786 inline friend bool operator!=(const Twist& a,const Twist& b);
00787
00788
00789 friend class Rotation;
00790 friend class Frame;
00791
00792 };
00793
00801
00802
00803
00804
00805
00806
00807
00809
00810
00811
00812
00813
00814
00816
00817
00820
00821
00822
00823
00824
00825
00826
00827
00828
00829
00830
00831
00832
00833
00834
00835
00836
00837
00838
00839
00840
00841
00842
00844
00845
00847
00848
00855
00856
00857
00860
00861
00863
00865
00866
00867
00868
00869
00870
00871
00872
00878 class Wrench
00879 {
00880 public:
00881 Vector force;
00882 Vector torque;
00883 public:
00884
00886 Wrench():force(),torque() {};
00887 Wrench(const Vector& _force,const Vector& _torque):force(_force),torque(_torque) {};
00888
00889
00890 inline Wrench& operator-=(const Wrench& arg);
00891 inline Wrench& operator+=(const Wrench& arg);
00892
00894 inline double& operator()(int i);
00895
00898 inline double operator()(int i) const;
00899
00900 double operator[] ( int index ) const
00901 {
00902 return this->operator() ( index );
00903 }
00904
00905 double& operator[] ( int index )
00906 {
00907 return this->operator() ( index );
00908 }
00909
00911 inline friend Wrench operator*(const Wrench& lhs,double rhs);
00913 inline friend Wrench operator*(double lhs,const Wrench& rhs);
00915 inline friend Wrench operator/(const Wrench& lhs,double rhs);
00916
00917 inline friend Wrench operator+(const Wrench& lhs,const Wrench& rhs);
00918 inline friend Wrench operator-(const Wrench& lhs,const Wrench& rhs);
00919
00921 inline friend Wrench operator-(const Wrench& arg);
00922
00925 inline friend void SetToZero(Wrench& v);
00926
00928 static inline Wrench Zero();
00929
00931 inline void ReverseSign();
00932
00939 inline Wrench RefPoint(const Vector& v_base_AB) const;
00940
00941
00944 inline friend bool Equal(const Wrench& a,const Wrench& b,double eps);
00945
00947 inline friend bool operator==(const Wrench& a,const Wrench& b);
00949 inline friend bool operator!=(const Wrench& a,const Wrench& b);
00950
00951 friend class Rotation;
00952 friend class Frame;
00953
00954
00955 };
00956
00957
00959 class Vector2
00960 {
00961 double data[2];
00962 public:
00964 Vector2() {data[0]=data[1] = 0.0;}
00965 inline Vector2(double x,double y);
00966 inline Vector2(const Vector2& arg);
00967
00968 inline Vector2& operator = ( const Vector2& arg);
00969
00971 inline double operator()(int index) const;
00972
00974 inline double& operator() (int index);
00975
00977 double operator[] ( int index ) const
00978 {
00979 return this->operator() ( index );
00980 }
00981
00983 double& operator[] ( int index )
00984 {
00985 return this->operator() ( index );
00986 }
00987
00988 inline double x() const;
00989 inline double y() const;
00990 inline void x(double);
00991 inline void y(double);
00992
00993 inline void ReverseSign();
00994 inline Vector2& operator-=(const Vector2& arg);
00995 inline Vector2& operator +=(const Vector2& arg);
00996
00997
00998 inline friend Vector2 operator*(const Vector2& lhs,double rhs);
00999 inline friend Vector2 operator*(double lhs,const Vector2& rhs);
01000 inline friend Vector2 operator/(const Vector2& lhs,double rhs);
01001 inline friend Vector2 operator+(const Vector2& lhs,const Vector2& rhs);
01002 inline friend Vector2 operator-(const Vector2& lhs,const Vector2& rhs);
01003 inline friend Vector2 operator*(const Vector2& lhs,const Vector2& rhs);
01004 inline friend Vector2 operator-(const Vector2& arg);
01005 inline friend void SetToZero(Vector2& v);
01006
01008 inline static Vector2 Zero();
01009
01015 double Normalize(double eps=epsilon);
01016
01018 double Norm() const;
01019
01021 inline void Set3DXY(const Vector& v);
01022
01024 inline void Set3DYZ(const Vector& v);
01025
01027 inline void Set3DZX(const Vector& v);
01028
01032 inline void Set3DPlane(const Frame& F_someframe_XY,const Vector& v_someframe);
01033
01034
01037 inline friend bool Equal(const Vector2& a,const Vector2& b,double eps);
01038
01040 inline friend bool operator==(const Vector2& a,const Vector2& b);
01042 inline friend bool operator!=(const Vector2& a,const Vector2& b);
01043
01044 friend class Rotation2;
01045 };
01046
01047
01050 class Rotation2
01051 {
01052 double s,c;
01055 public:
01057 Rotation2() {c=1.0;s=0.0;}
01058
01059 explicit Rotation2(double angle_rad):s(sin(angle_rad)),c(cos(angle_rad)) {}
01060
01061 Rotation2(double ca,double sa):s(sa),c(ca){}
01062
01063 inline Rotation2& operator=(const Rotation2& arg);
01064 inline Vector2 operator*(const Vector2& v) const;
01066 inline double operator() (int i,int j) const;
01067
01068 inline friend Rotation2 operator *(const Rotation2& lhs,const Rotation2& rhs);
01069
01070 inline void SetInverse();
01071 inline Rotation2 Inverse() const;
01072 inline Vector2 Inverse(const Vector2& v) const;
01073
01074 inline void SetIdentity();
01075 inline static Rotation2 Identity();
01076
01077
01079 inline void SetRot(double angle);
01080
01082 inline static Rotation2 Rot(double angle);
01083
01085 inline double GetRot() const;
01086
01089 inline friend bool Equal(const Rotation2& a,const Rotation2& b,double eps);
01090 };
01091
01094 class Frame2
01095 {
01096 public:
01097 Vector2 p;
01098 Rotation2 M;
01099
01100 public:
01101
01102 inline Frame2(const Rotation2& R,const Vector2& V);
01103 explicit inline Frame2(const Vector2& V);
01104 explicit inline Frame2(const Rotation2& R);
01105 inline Frame2(void);
01106 inline Frame2(const Frame2& arg);
01107 inline void Make4x4(double* d);
01108
01111 inline double operator()(int i,int j);
01112
01115 inline double operator() (int i,int j) const;
01116
01117 inline void SetInverse();
01118 inline Frame2 Inverse() const;
01119 inline Vector2 Inverse(const Vector2& arg) const;
01120 inline Frame2& operator = (const Frame2& arg);
01121 inline Vector2 operator * (const Vector2& arg);
01122 inline friend Frame2 operator *(const Frame2& lhs,const Frame2& rhs);
01123 inline void SetIdentity();
01124 inline void Integrate(const Twist& t_this,double frequency);
01125 inline static Frame2 Identity() {
01126 Frame2 tmp;
01127 tmp.SetIdentity();
01128 return tmp;
01129 }
01130 inline friend bool Equal(const Frame2& a,const Frame2& b,double eps);
01131 };
01132
01143 IMETHOD Vector diff(const Vector& p_w_a,const Vector& p_w_b,double dt=1);
01144
01145
01173 IMETHOD Vector diff(const Rotation& R_a_b1,const Rotation& R_a_b2,double dt=1);
01174
01183 IMETHOD Twist diff(const Frame& F_a_b1,const Frame& F_a_b2,double dt=1);
01184
01189 IMETHOD Twist diff(const Twist& a,const Twist& b,double dt=1);
01190
01195 IMETHOD Wrench diff(const Wrench& W_a_p1,const Wrench& W_a_p2,double dt=1);
01196
01204 IMETHOD Vector addDelta(const Vector& p_w_a,const Vector& p_w_da,double dt=1);
01205
01218 IMETHOD Rotation addDelta(const Rotation& R_w_a,const Vector& da_w,double dt=1);
01219
01230 IMETHOD Frame addDelta(const Frame& F_w_a,const Twist& da_w,double dt=1);
01231
01239 IMETHOD Twist addDelta(const Twist& a,const Twist&da,double dt=1);
01240
01241
01250 IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt=1);
01251
01252 #ifdef KDL_INLINE
01253
01254
01255
01256
01257
01258
01259
01260
01261 #include "frames.inl"
01262 #endif
01263
01264
01265
01266 }
01267
01268
01269 #endif