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
00151 class Vector
00152 {
00153 public:
00154 double data[3];
00156 inline Vector() {data[0]=data[1]=data[2] = 0.0;}
00157
00159 inline Vector(double x,double y, double z);
00160
00162 inline Vector(const Vector& arg);
00163
00165 inline Vector& operator = ( const Vector& arg);
00166
00168 inline double operator()(int index) const;
00169
00171 inline double& operator() (int index);
00172
00174 double operator[] ( int index ) const
00175 {
00176 return this->operator() ( index );
00177 }
00178
00180 double& operator[] ( int index )
00181 {
00182 return this->operator() ( index );
00183 }
00184
00185 inline double x() const;
00186 inline double y() const;
00187 inline double z() const;
00188 inline void x(double);
00189 inline void y(double);
00190 inline void z(double);
00191
00193 inline void ReverseSign();
00194
00195
00197 inline Vector& operator-=(const Vector& arg);
00198
00199
00201 inline Vector& operator +=(const Vector& arg);
00202
00204 inline friend Vector operator*(const Vector& lhs,double rhs);
00206 inline friend Vector operator*(double lhs,const Vector& rhs);
00208
00209 inline friend Vector operator/(const Vector& lhs,double rhs);
00210 inline friend Vector operator+(const Vector& lhs,const Vector& rhs);
00211 inline friend Vector operator-(const Vector& lhs,const Vector& rhs);
00212 inline friend Vector operator*(const Vector& lhs,const Vector& rhs);
00213 inline friend Vector operator-(const Vector& arg);
00214 inline friend double dot(const Vector& lhs,const Vector& rhs);
00215
00218 inline friend void SetToZero(Vector& v);
00219
00221 inline static Vector Zero();
00222
00228 double Normalize(double eps=epsilon);
00229
00231 double Norm() const;
00232
00233
00234
00236 inline void Set2DXY(const Vector2& v);
00238 inline void Set2DYZ(const Vector2& v);
00240 inline void Set2DZX(const Vector2& v);
00242 inline void Set2DPlane(const Frame& F_someframe_XY,const Vector2& v_XY);
00243
00244
00247 inline friend bool Equal(const Vector& a,const Vector& b,double eps=epsilon);
00248
00250 inline friend bool operator==(const Vector& a,const Vector& b);
00252 inline friend bool operator!=(const Vector& a,const Vector& b);
00253
00254 friend class Rotation;
00255 friend class Frame;
00256 };
00257
00258
00292 class Rotation
00293 {
00294 public:
00295 double data[9];
00296
00297 inline Rotation() {
00298 *this = Rotation::Identity();
00299 }
00300 inline Rotation(double Xx,double Yx,double Zx,
00301 double Xy,double Yy,double Zy,
00302 double Xz,double Yz,double Zz);
00303 inline Rotation(const Vector& x,const Vector& y,const Vector& z);
00304
00305
00306
00307 inline Rotation& operator=(const Rotation& arg);
00308
00311 inline Vector operator*(const Vector& v) const;
00312
00314 inline double& operator()(int i,int j);
00315
00317 inline double operator() (int i,int j) const;
00318
00319 friend Rotation operator *(const Rotation& lhs,const Rotation& rhs);
00320
00322 inline void SetInverse();
00323
00325 inline Rotation Inverse() const;
00326
00328 inline Vector Inverse(const Vector& v) const;
00329
00331 inline Wrench Inverse(const Wrench& arg) const;
00332
00334 inline Twist Inverse(const Twist& arg) const;
00335
00337 inline static Rotation Identity();
00338
00339
00340
00342 inline static Rotation RotX(double angle);
00344 inline static Rotation RotY(double angle);
00346 inline static Rotation RotZ(double angle);
00349 inline void DoRotX(double angle);
00352 inline void DoRotY(double angle);
00355 inline void DoRotZ(double angle);
00356
00360
00361 static Rotation Rot(const Vector& rotvec,double angle);
00362
00364 static Rotation Rot2(const Vector& rotvec,double angle);
00365
00368 Vector GetRot() const;
00369
00378 double GetRotAngle(Vector& axis,double eps=epsilon) const;
00379
00380
00389 static Rotation EulerZYZ(double Alfa,double Beta,double Gamma);
00390
00406 void GetEulerZYZ(double& alpha,double& beta,double& gamma) const;
00407
00410 static Rotation Quaternion(double x,double y,double z, double w);
00411
00414 void GetQuaternion(double& x,double& y,double& z, double& w) const;
00415
00426 static Rotation RPY(double roll,double pitch,double yaw);
00427
00446 void GetRPY(double& roll,double& pitch,double& yaw) const;
00447
00448
00460 inline static Rotation EulerZYX(double Alfa,double Beta,double Gamma) {
00461 return RPY(Gamma,Beta,Alfa);
00462 }
00463
00484 inline void GetEulerZYX(double& Alfa,double& Beta,double& Gamma) const {
00485 GetRPY(Gamma,Beta,Alfa);
00486 }
00487
00492 inline Twist operator * (const Twist& arg) const;
00493
00498 inline Wrench operator * (const Wrench& arg) const;
00499
00501 inline Vector UnitX() const {
00502 return Vector(data[0],data[3],data[6]);
00503 }
00504
00506 inline void UnitX(const Vector& X) {
00507 data[0] = X(0);
00508 data[3] = X(1);
00509 data[6] = X(2);
00510 }
00511
00513 inline Vector UnitY() const {
00514 return Vector(data[1],data[4],data[7]);
00515 }
00516
00518 inline void UnitY(const Vector& X) {
00519 data[1] = X(0);
00520 data[4] = X(1);
00521 data[7] = X(2);
00522 }
00523
00525 inline Vector UnitZ() const {
00526 return Vector(data[2],data[5],data[8]);
00527 }
00528
00530 inline void UnitZ(const Vector& X) {
00531 data[2] = X(0);
00532 data[5] = X(1);
00533 data[8] = X(2);
00534 }
00535
00538 friend bool Equal(const Rotation& a,const Rotation& b,double eps);
00539
00541 friend bool operator==(const Rotation& a,const Rotation& b);
00543 friend bool operator!=(const Rotation& a,const Rotation& b);
00544
00545 friend class Frame;
00546 };
00547 bool operator==(const Rotation& a,const Rotation& b);
00548 bool Equal(const Rotation& a,const Rotation& b,double eps=epsilon);
00549
00550
00551
00561 class Frame {
00562 public:
00563 Vector p;
00564 Rotation M;
00565
00566 public:
00567
00568 inline Frame(const Rotation& R,const Vector& V);
00569
00571 explicit inline Frame(const Vector& V);
00573 explicit inline Frame(const Rotation& R);
00574
00575 inline Frame() {}
00577 inline Frame(const Frame& arg);
00578
00580
00581 void Make4x4(double* d);
00582
00585 inline double operator()(int i,int j);
00586
00589 inline double operator() (int i,int j) const;
00590
00591
00593 inline Frame Inverse() const;
00594
00596 inline Vector Inverse(const Vector& arg) const;
00597
00599 inline Wrench Inverse(const Wrench& arg) const;
00600
00602 inline Twist Inverse(const Twist& arg) const;
00603
00605 inline Frame& operator = (const Frame& arg);
00606
00609 inline Vector operator * (const Vector& arg) const;
00610
00617 inline Wrench operator * (const Wrench& arg) const;
00618
00625 inline Twist operator * (const Twist& arg) const;
00626
00628 inline friend Frame operator *(const Frame& lhs,const Frame& rhs);
00629
00631 inline static Frame Identity();
00632
00636 inline void Integrate(const Twist& t_this,double frequency);
00637
00638
00639
00640
00641
00642
00643
00644
00645
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 static Frame DH_Craig1989(double a,double alpha,double d,double theta);
00685
00686
00687
00688
00689
00690
00691
00692 static Frame DH(double a,double alpha,double d,double theta);
00693
00694
00697 inline friend bool Equal(const Frame& a,const Frame& b,double eps=epsilon);
00698
00700 inline friend bool operator==(const Frame& a,const Frame& b);
00702 inline friend bool operator!=(const Frame& a,const Frame& b);
00703 };
00704
00711 class Twist {
00712 public:
00713 Vector vel;
00714 Vector rot;
00715 public:
00716
00718 Twist():vel(),rot() {};
00719
00720 Twist(const Vector& _vel,const Vector& _rot):vel(_vel),rot(_rot) {};
00721
00722 inline Twist& operator-=(const Twist& arg);
00723 inline Twist& operator+=(const Twist& arg);
00725 inline double& operator()(int i);
00726
00729 inline double operator()(int i) const;
00730
00731 double operator[] ( int index ) const
00732 {
00733 return this->operator() ( index );
00734 }
00735
00736 double& operator[] ( int index )
00737 {
00738 return this->operator() ( index );
00739 }
00740
00741 inline friend Twist operator*(const Twist& lhs,double rhs);
00742 inline friend Twist operator*(double lhs,const Twist& rhs);
00743 inline friend Twist operator/(const Twist& lhs,double rhs);
00744 inline friend Twist operator+(const Twist& lhs,const Twist& rhs);
00745 inline friend Twist operator-(const Twist& lhs,const Twist& rhs);
00746 inline friend Twist operator-(const Twist& arg);
00747 inline friend double dot(const Twist& lhs,const Wrench& rhs);
00748 inline friend double dot(const Wrench& rhs,const Twist& lhs);
00749 inline friend void SetToZero(Twist& v);
00751 inline friend Twist operator*(const Twist& lhs,const Twist& rhs);
00753 inline friend Wrench operator*(const Twist& lhs,const Wrench& rhs);
00754
00756 static inline Twist Zero();
00757
00759 inline void ReverseSign();
00760
00767 inline Twist RefPoint(const Vector& v_base_AB) const;
00768
00769
00772 inline friend bool Equal(const Twist& a,const Twist& b,double eps=epsilon);
00773
00775 inline friend bool operator==(const Twist& a,const Twist& b);
00777 inline friend bool operator!=(const Twist& a,const Twist& b);
00778
00779
00780 friend class Rotation;
00781 friend class Frame;
00782
00783 };
00784
00792
00793
00794
00795
00796
00797
00798
00800
00801
00802
00803
00804
00805
00807
00808
00811
00812
00813
00814
00815
00816
00817
00818
00819
00820
00821
00822
00823
00824
00825
00826
00827
00828
00829
00830
00831
00832
00833
00835
00836
00838
00839
00846
00847
00848
00851
00852
00854
00856
00857
00858
00859
00860
00861
00862
00863
00869 class Wrench
00870 {
00871 public:
00872 Vector force;
00873 Vector torque;
00874 public:
00875
00877 Wrench():force(),torque() {};
00878 Wrench(const Vector& _force,const Vector& _torque):force(_force),torque(_torque) {};
00879
00880
00881 inline Wrench& operator-=(const Wrench& arg);
00882 inline Wrench& operator+=(const Wrench& arg);
00883
00885 inline double& operator()(int i);
00886
00889 inline double operator()(int i) const;
00890
00891 double operator[] ( int index ) const
00892 {
00893 return this->operator() ( index );
00894 }
00895
00896 double& operator[] ( int index )
00897 {
00898 return this->operator() ( index );
00899 }
00900
00902 inline friend Wrench operator*(const Wrench& lhs,double rhs);
00904 inline friend Wrench operator*(double lhs,const Wrench& rhs);
00906 inline friend Wrench operator/(const Wrench& lhs,double rhs);
00907
00908 inline friend Wrench operator+(const Wrench& lhs,const Wrench& rhs);
00909 inline friend Wrench operator-(const Wrench& lhs,const Wrench& rhs);
00910
00912 inline friend Wrench operator-(const Wrench& arg);
00913
00916 inline friend void SetToZero(Wrench& v);
00917
00919 static inline Wrench Zero();
00920
00922 inline void ReverseSign();
00923
00930 inline Wrench RefPoint(const Vector& v_base_AB) const;
00931
00932
00935 inline friend bool Equal(const Wrench& a,const Wrench& b,double eps=epsilon);
00936
00938 inline friend bool operator==(const Wrench& a,const Wrench& b);
00940 inline friend bool operator!=(const Wrench& a,const Wrench& b);
00941
00942 friend class Rotation;
00943 friend class Frame;
00944
00945
00946 };
00947
00948
00950 class Vector2
00951 {
00952 double data[2];
00953 public:
00955 Vector2() {data[0]=data[1] = 0.0;}
00956 inline Vector2(double x,double y);
00957 inline Vector2(const Vector2& arg);
00958
00959 inline Vector2& operator = ( const Vector2& arg);
00960
00962 inline double operator()(int index) const;
00963
00965 inline double& operator() (int index);
00966
00968 double operator[] ( int index ) const
00969 {
00970 return this->operator() ( index );
00971 }
00972
00974 double& operator[] ( int index )
00975 {
00976 return this->operator() ( index );
00977 }
00978
00979 inline double x() const;
00980 inline double y() const;
00981 inline void x(double);
00982 inline void y(double);
00983
00984 inline void ReverseSign();
00985 inline Vector2& operator-=(const Vector2& arg);
00986 inline Vector2& operator +=(const Vector2& arg);
00987
00988
00989 inline friend Vector2 operator*(const Vector2& lhs,double rhs);
00990 inline friend Vector2 operator*(double lhs,const Vector2& rhs);
00991 inline friend Vector2 operator/(const Vector2& lhs,double rhs);
00992 inline friend Vector2 operator+(const Vector2& lhs,const Vector2& rhs);
00993 inline friend Vector2 operator-(const Vector2& lhs,const Vector2& rhs);
00994 inline friend Vector2 operator*(const Vector2& lhs,const Vector2& rhs);
00995 inline friend Vector2 operator-(const Vector2& arg);
00996 inline friend void SetToZero(Vector2& v);
00997
00999 inline static Vector2 Zero();
01000
01006 double Normalize(double eps=epsilon);
01007
01009 double Norm() const;
01010
01012 inline void Set3DXY(const Vector& v);
01013
01015 inline void Set3DYZ(const Vector& v);
01016
01018 inline void Set3DZX(const Vector& v);
01019
01023 inline void Set3DPlane(const Frame& F_someframe_XY,const Vector& v_someframe);
01024
01025
01028 inline friend bool Equal(const Vector2& a,const Vector2& b,double eps=epsilon);
01029
01031 inline friend bool operator==(const Vector2& a,const Vector2& b);
01033 inline friend bool operator!=(const Vector2& a,const Vector2& b);
01034
01035 friend class Rotation2;
01036 };
01037
01038
01041 class Rotation2
01042 {
01043 double s,c;
01046 public:
01048 Rotation2() {c=1.0;s=0.0;}
01049
01050 explicit Rotation2(double angle_rad):s(sin(angle_rad)),c(cos(angle_rad)) {}
01051
01052 Rotation2(double ca,double sa):s(sa),c(ca){}
01053
01054 inline Rotation2& operator=(const Rotation2& arg);
01055 inline Vector2 operator*(const Vector2& v) const;
01057 inline double operator() (int i,int j) const;
01058
01059 inline friend Rotation2 operator *(const Rotation2& lhs,const Rotation2& rhs);
01060
01061 inline void SetInverse();
01062 inline Rotation2 Inverse() const;
01063 inline Vector2 Inverse(const Vector2& v) const;
01064
01065 inline void SetIdentity();
01066 inline static Rotation2 Identity();
01067
01068
01070 inline void SetRot(double angle);
01071
01073 inline static Rotation2 Rot(double angle);
01074
01076 inline double GetRot() const;
01077
01080 inline friend bool Equal(const Rotation2& a,const Rotation2& b,double eps=epsilon);
01081 };
01082
01085 class Frame2
01086 {
01087 public:
01088 Vector2 p;
01089 Rotation2 M;
01090
01091 public:
01092
01093 inline Frame2(const Rotation2& R,const Vector2& V);
01094 explicit inline Frame2(const Vector2& V);
01095 explicit inline Frame2(const Rotation2& R);
01096 inline Frame2(void);
01097 inline Frame2(const Frame2& arg);
01098 inline void Make4x4(double* d);
01099
01102 inline double operator()(int i,int j);
01103
01106 inline double operator() (int i,int j) const;
01107
01108 inline void SetInverse();
01109 inline Frame2 Inverse() const;
01110 inline Vector2 Inverse(const Vector2& arg) const;
01111 inline Frame2& operator = (const Frame2& arg);
01112 inline Vector2 operator * (const Vector2& arg);
01113 inline friend Frame2 operator *(const Frame2& lhs,const Frame2& rhs);
01114 inline void SetIdentity();
01115 inline void Integrate(const Twist& t_this,double frequency);
01116 inline static Frame2 Identity() {
01117 Frame2 tmp;
01118 tmp.SetIdentity();
01119 return tmp;
01120 }
01121 inline friend bool Equal(const Frame2& a,const Frame2& b,double eps=epsilon);
01122 };
01123
01134 IMETHOD Vector diff(const Vector& p_w_a,const Vector& p_w_b,double dt=1);
01135
01136
01164 IMETHOD Vector diff(const Rotation& R_a_b1,const Rotation& R_a_b2,double dt=1);
01165
01174 IMETHOD Twist diff(const Frame& F_a_b1,const Frame& F_a_b2,double dt=1);
01175
01180 IMETHOD Twist diff(const Twist& a,const Twist& b,double dt=1);
01181
01186 IMETHOD Wrench diff(const Wrench& W_a_p1,const Wrench& W_a_p2,double dt=1);
01187
01195 IMETHOD Vector addDelta(const Vector& p_w_a,const Vector& p_w_da,double dt=1);
01196
01209 IMETHOD Rotation addDelta(const Rotation& R_w_a,const Vector& da_w,double dt=1);
01210
01221 IMETHOD Frame addDelta(const Frame& F_w_a,const Twist& da_w,double dt=1);
01222
01230 IMETHOD Twist addDelta(const Twist& a,const Twist&da,double dt=1);
01231
01232
01241 IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt=1);
01242
01243 #ifdef KDL_INLINE
01244
01245
01246
01247
01248
01249
01250
01251
01252 #include "frames.inl"
01253 #endif
01254
01255
01256
01257 }
01258
01259
01260 #endif