$search
00001 /*************************************************************************** 00002 frames.hpp `- description 00003 ------------------------- 00004 begin : June 2006 00005 copyright : (C) 2006 Erwin Aertbelien 00006 email : firstname.lastname@mech.kuleuven.be 00007 00008 History (only major changes)( AUTHOR-Description ) : 00009 00010 *************************************************************************** 00011 * This library is free software; you can redistribute it and/or * 00012 * modify it under the terms of the GNU Lesser General Public * 00013 * License as published by the Free Software Foundation; either * 00014 * version 2.1 of the License, or (at your option) any later version. * 00015 * * 00016 * This library is distributed in the hope that it will be useful, * 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of * 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 00019 * Lesser General Public License for more details. * 00020 * * 00021 * You should have received a copy of the GNU Lesser General Public * 00022 * License along with this library; if not, write to the Free Software * 00023 * Foundation, Inc., 59 Temple Place, * 00024 * Suite 330, Boston, MA 02111-1307 USA * 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 // default copy constructor is sufficient 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 // = Rotations 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 // @see Rot2 if you want to handle this error in another way. 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 //\TODO should be formulated as a constructor 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 // = Inverse 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 // DH_Craig1989 : constructs a transformationmatrix 00640 // T_link(i-1)_link(i) with the Denavit-Hartenberg convention as 00641 // described in the Craigs book: Craig, J. J.,Introduction to 00642 // Robotics: Mechanics and Control, Addison-Wesley, 00643 // isbn:0-201-10326-5, 1986. 00644 // 00645 // Note that the frame is a redundant way to express the information 00646 // in the DH-convention. 00647 // \verbatim 00648 // Parameters in full : a(i-1),alpha(i-1),d(i),theta(i) 00649 // 00650 // axis i-1 is connected by link i-1 to axis i numbering axis 1 00651 // to axis n link 0 (immobile base) to link n 00652 // 00653 // link length a(i-1) length of the mutual perpendicular line 00654 // (normal) between the 2 axes. This normal runs from (i-1) to 00655 // (i) axis. 00656 // 00657 // link twist alpha(i-1): construct plane perpendicular to the 00658 // normal project axis(i-1) and axis(i) into plane angle from 00659 // (i-1) to (i) measured in the direction of the normal 00660 // 00661 // link offset d(i) signed distance between normal (i-1) to (i) 00662 // and normal (i) to (i+1) along axis i joint angle theta(i) 00663 // signed angle between normal (i-1) to (i) and normal (i) to 00664 // (i+1) along axis i 00665 // 00666 // First and last joints : a(0)= a(n) = 0 00667 // alpha(0) = alpha(n) = 0 00668 // 00669 // PRISMATIC : theta(1) = 0 d(1) arbitrarily 00670 // 00671 // REVOLUTE : theta(1) arbitrarily d(1) = 0 00672 // 00673 // Not unique : if intersecting joint axis 2 choices for normal 00674 // Frame assignment of the DH convention : Z(i-1) follows axis 00675 // (i-1) X(i-1) is the normal between axis(i-1) and axis(i) 00676 // Y(i-1) follows out of Z(i-1) and X(i-1) 00677 // 00678 // a(i-1) = distance from Z(i-1) to Z(i) along X(i-1) 00679 // alpha(i-1) = angle between Z(i-1) to Z(i) along X(i-1) 00680 // d(i) = distance from X(i-1) to X(i) along Z(i) 00681 // theta(i) = angle between X(i-1) to X(i) along X(i) 00682 // \endverbatim 00683 */ 00684 static Frame DH_Craig1989(double a,double alpha,double d,double theta); 00685 00686 // DH : constructs a transformationmatrix T_link(i-1)_link(i) with 00687 // the Denavit-Hartenberg convention as described in the original 00688 // publictation: Denavit, J. and Hartenberg, R. S., A kinematic 00689 // notation for lower-pair mechanisms based on matrices, ASME 00690 // Journal of Applied Mechanics, 23:215-221, 1955. 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 // = Friends 00780 friend class Rotation; 00781 friend class Frame; 00782 00783 }; 00784 00792 /* 00793 class AccelerationTwist { 00794 public: 00795 Vector trans; //!< The translational acceleration of that point 00796 Vector rot; //!< The rotational acceleration of that point. 00797 public: 00798 00800 AccelerationTwist():trans(),rot() {}; 00801 00802 AccelerationTwist(const Vector& _trans,const Vector& _rot):trans(_trans),rot(_rot) {}; 00803 00804 inline AccelerationTwist& operator-=(const AccelerationTwist& arg); 00805 inline AccelerationTwist& operator+=(const AccelerationTwist& arg); 00807 inline double& operator()(int i); 00808 00811 inline double operator()(int i) const; 00812 00813 double operator[] ( int index ) const 00814 { 00815 return this->operator() ( index ); 00816 } 00817 00818 double& operator[] ( int index ) 00819 { 00820 return this->operator() ( index ); 00821 } 00822 00823 inline friend AccelerationTwist operator*(const AccelerationTwist& lhs,double rhs); 00824 inline friend AccelerationTwist operator*(double lhs,const AccelerationTwist& rhs); 00825 inline friend AccelerationTwist operator/(const AccelerationTwist& lhs,double rhs); 00826 inline friend AccelerationTwist operator+(const AccelerationTwist& lhs,const AccelerationTwist& rhs); 00827 inline friend AccelerationTwist operator-(const AccelerationTwist& lhs,const AccelerationTwist& rhs); 00828 inline friend AccelerationTwist operator-(const AccelerationTwist& arg); 00829 //inline friend double dot(const AccelerationTwist& lhs,const Wrench& rhs); 00830 //inline friend double dot(const Wrench& rhs,const AccelerationTwist& lhs); 00831 inline friend void SetToZero(AccelerationTwist& v); 00832 00833 00835 static inline AccelerationTwist Zero(); 00836 00838 inline void ReverseSign(); 00839 00846 inline AccelerationTwist RefPoint(const Vector& v_base_AB) const; 00847 00848 00851 inline friend bool Equal(const AccelerationTwist& a,const AccelerationTwist& b,double eps=epsilon); 00852 00854 inline friend bool operator==(const AccelerationTwist& a,const AccelerationTwist& b); 00856 inline friend bool operator!=(const AccelerationTwist& a,const AccelerationTwist& b); 00857 00858 // = Friends 00859 friend class Rotation; 00860 friend class Frame; 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 // = Operators 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 // #include "vector.inl" 01245 // #include "wrench.inl" 01246 //#include "rotation.inl" 01247 //#include "frame.inl" 01248 //#include "twist.inl" 01249 //#include "vector2.inl" 01250 //#include "rotation2.inl" 01251 //#include "frame2.inl" 01252 #include "frames.inl" 01253 #endif 01254 01255 01256 01257 } 01258 01259 01260 #endif