KDL  1.4.0
frames.hpp
Go to the documentation of this file.
1 /***************************************************************************
2  frames.hpp `- description
3  -------------------------
4  begin : June 2006
5  copyright : (C) 2006 Erwin Aertbelien
6  email : firstname.lastname@mech.kuleuven.be
7 
8  History (only major changes)( AUTHOR-Description ) :
9 
10  ***************************************************************************
11  * This library is free software; you can redistribute it and/or *
12  * modify it under the terms of the GNU Lesser General Public *
13  * License as published by the Free Software Foundation; either *
14  * version 2.1 of the License, or (at your option) any later version. *
15  * *
16  * This library is distributed in the hope that it will be useful, *
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *
19  * Lesser General Public License for more details. *
20  * *
21  * You should have received a copy of the GNU Lesser General Public *
22  * License along with this library; if not, write to the Free Software *
23  * Foundation, Inc., 59 Temple Place, *
24  * Suite 330, Boston, MA 02111-1307 USA *
25  * *
26  ***************************************************************************/
27 
124 #ifndef KDL_FRAMES_H
125 #define KDL_FRAMES_H
126 
127 
128 #include "utilities/kdl-config.h"
129 #include "utilities/utility.h"
130 
132 
133 namespace KDL {
134 
135 
136 
137 class Vector;
138 class Rotation;
139 class Frame;
140 class Wrench;
141 class Twist;
142 class Vector2;
143 class Rotation2;
144 class Frame2;
145 
146 
147 // Equal is friend function, but default arguments for friends are forbidden (ยง8.3.6.4)
148 inline bool Equal(const Vector& a,const Vector& b,double eps=epsilon);
149 inline bool Equal(const Frame& a,const Frame& b,double eps=epsilon);
150 inline bool Equal(const Twist& a,const Twist& b,double eps=epsilon);
151 inline bool Equal(const Wrench& a,const Wrench& b,double eps=epsilon);
152 inline bool Equal(const Vector2& a,const Vector2& b,double eps=epsilon);
153 inline bool Equal(const Rotation2& a,const Rotation2& b,double eps=epsilon);
154 inline bool Equal(const Frame2& a,const Frame2& b,double eps=epsilon);
155 
156 
160 class Vector
161 {
162 public:
163  double data[3];
165  inline Vector() {data[0]=data[1]=data[2] = 0.0;}
166 
168  inline Vector(double x,double y, double z);
169 
171  inline Vector(const Vector& arg);
172 
174  inline Vector& operator = ( const Vector& arg);
175 
177  inline double operator()(int index) const;
178 
180  inline double& operator() (int index);
181 
183  double operator[] ( int index ) const
184  {
185  return this->operator() ( index );
186  }
187 
189  double& operator[] ( int index )
190  {
191  return this->operator() ( index );
192  }
193 
194  inline double x() const;
195  inline double y() const;
196  inline double z() const;
197  inline void x(double);
198  inline void y(double);
199  inline void z(double);
200 
202  inline void ReverseSign();
203 
204 
206  inline Vector& operator-=(const Vector& arg);
207 
208 
210  inline Vector& operator +=(const Vector& arg);
211 
213  inline friend Vector operator*(const Vector& lhs,double rhs);
215  inline friend Vector operator*(double lhs,const Vector& rhs);
217 
218  inline friend Vector operator/(const Vector& lhs,double rhs);
219  inline friend Vector operator+(const Vector& lhs,const Vector& rhs);
220  inline friend Vector operator-(const Vector& lhs,const Vector& rhs);
221  inline friend Vector operator*(const Vector& lhs,const Vector& rhs);
222  inline friend Vector operator-(const Vector& arg);
223  inline friend double dot(const Vector& lhs,const Vector& rhs);
224 
227  inline friend void SetToZero(Vector& v);
228 
230  inline static Vector Zero();
231 
237  double Normalize(double eps=epsilon);
238 
240  double Norm() const;
241 
242 
243 
245  inline void Set2DXY(const Vector2& v);
247  inline void Set2DYZ(const Vector2& v);
249  inline void Set2DZX(const Vector2& v);
251  inline void Set2DPlane(const Frame& F_someframe_XY,const Vector2& v_XY);
252 
253 
256  inline friend bool Equal(const Vector& a,const Vector& b,double eps);
257 
259  inline friend bool operator==(const Vector& a,const Vector& b);
261  inline friend bool operator!=(const Vector& a,const Vector& b);
262 
263  friend class Rotation;
264  friend class Frame;
265 };
266 
267 
301 class Rotation
302 {
303 public:
304  double data[9];
305 
306  inline Rotation() {
307  *this = Rotation::Identity();
308  }
309  inline Rotation(double Xx,double Yx,double Zx,
310  double Xy,double Yy,double Zy,
311  double Xz,double Yz,double Zz);
312  inline Rotation(const Vector& x,const Vector& y,const Vector& z);
313  // default copy constructor is sufficient
314 
315 
316  inline Rotation& operator=(const Rotation& arg);
317 
320  inline Vector operator*(const Vector& v) const;
321 
323  inline double& operator()(int i,int j);
324 
326  inline double operator() (int i,int j) const;
327 
328  friend Rotation operator *(const Rotation& lhs,const Rotation& rhs);
329 
331  inline void SetInverse();
332 
334  inline Rotation Inverse() const;
335 
337  inline Vector Inverse(const Vector& v) const;
338 
340  inline Wrench Inverse(const Wrench& arg) const;
341 
343  inline Twist Inverse(const Twist& arg) const;
344 
346  inline static Rotation Identity();
347 
348 
349 // = Rotations
351  inline static Rotation RotX(double angle);
353  inline static Rotation RotY(double angle);
355  inline static Rotation RotZ(double angle);
358  inline void DoRotX(double angle);
361  inline void DoRotY(double angle);
364  inline void DoRotZ(double angle);
365 
369  // @see Rot2 if you want to handle this error in another way.
370  static Rotation Rot(const Vector& rotvec,double angle);
371 
373  static Rotation Rot2(const Vector& rotvec,double angle);
374 
377  Vector GetRot() const;
378 
387  double GetRotAngle(Vector& axis,double eps=epsilon) const;
388 
389 
398  static Rotation EulerZYZ(double Alfa,double Beta,double Gamma);
399 
415  void GetEulerZYZ(double& alpha,double& beta,double& gamma) const;
416 
419  static Rotation Quaternion(double x,double y,double z, double w);
420 
423  void GetQuaternion(double& x,double& y,double& z, double& w) const;
424 
435  static Rotation RPY(double roll,double pitch,double yaw);
436 
455  void GetRPY(double& roll,double& pitch,double& yaw) const;
456 
457 
469  inline static Rotation EulerZYX(double Alfa,double Beta,double Gamma) {
470  return RPY(Gamma,Beta,Alfa);
471  }
472 
493  inline void GetEulerZYX(double& Alfa,double& Beta,double& Gamma) const {
494  GetRPY(Gamma,Beta,Alfa);
495  }
496 
501  inline Twist operator * (const Twist& arg) const;
502 
507  inline Wrench operator * (const Wrench& arg) const;
508 
510  inline Vector UnitX() const {
511  return Vector(data[0],data[3],data[6]);
512  }
513 
515  inline void UnitX(const Vector& X) {
516  data[0] = X(0);
517  data[3] = X(1);
518  data[6] = X(2);
519  }
520 
522  inline Vector UnitY() const {
523  return Vector(data[1],data[4],data[7]);
524  }
525 
527  inline void UnitY(const Vector& X) {
528  data[1] = X(0);
529  data[4] = X(1);
530  data[7] = X(2);
531  }
532 
534  inline Vector UnitZ() const {
535  return Vector(data[2],data[5],data[8]);
536  }
537 
539  inline void UnitZ(const Vector& X) {
540  data[2] = X(0);
541  data[5] = X(1);
542  data[8] = X(2);
543  }
544 
547  friend bool Equal(const Rotation& a,const Rotation& b,double eps);
548 
550  friend bool operator==(const Rotation& a,const Rotation& b);
552  friend bool operator!=(const Rotation& a,const Rotation& b);
553 
554  friend class Frame;
555 };
556  bool operator==(const Rotation& a,const Rotation& b);
557  bool Equal(const Rotation& a,const Rotation& b,double eps=epsilon);
558 
559 
560 
570 class Frame {
571 public:
574 
575 public:
576 
577  inline Frame(const Rotation& R,const Vector& V);
578 
580  explicit inline Frame(const Vector& V);
582  explicit inline Frame(const Rotation& R);
583 
584  inline Frame() {}
586  inline Frame(const Frame& arg);
587 
589  //\TODO should be formulated as a constructor
590  void Make4x4(double* d);
591 
594  inline double operator()(int i,int j);
595 
598  inline double operator() (int i,int j) const;
599 
600 // = Inverse
602  inline Frame Inverse() const;
603 
605  inline Vector Inverse(const Vector& arg) const;
606 
608  inline Wrench Inverse(const Wrench& arg) const;
609 
611  inline Twist Inverse(const Twist& arg) const;
612 
614  inline Frame& operator = (const Frame& arg);
615 
618  inline Vector operator * (const Vector& arg) const;
619 
626  inline Wrench operator * (const Wrench& arg) const;
627 
634  inline Twist operator * (const Twist& arg) const;
635 
637  inline friend Frame operator *(const Frame& lhs,const Frame& rhs);
638 
640  inline static Frame Identity();
641 
645  inline void Integrate(const Twist& t_this,double frequency);
646 
647  /*
648  // DH_Craig1989 : constructs a transformationmatrix
649  // T_link(i-1)_link(i) with the Denavit-Hartenberg convention as
650  // described in the Craigs book: Craig, J. J.,Introduction to
651  // Robotics: Mechanics and Control, Addison-Wesley,
652  // isbn:0-201-10326-5, 1986.
653  //
654  // Note that the frame is a redundant way to express the information
655  // in the DH-convention.
656  // \verbatim
657  // Parameters in full : a(i-1),alpha(i-1),d(i),theta(i)
658  //
659  // axis i-1 is connected by link i-1 to axis i numbering axis 1
660  // to axis n link 0 (immobile base) to link n
661  //
662  // link length a(i-1) length of the mutual perpendicular line
663  // (normal) between the 2 axes. This normal runs from (i-1) to
664  // (i) axis.
665  //
666  // link twist alpha(i-1): construct plane perpendicular to the
667  // normal project axis(i-1) and axis(i) into plane angle from
668  // (i-1) to (i) measured in the direction of the normal
669  //
670  // link offset d(i) signed distance between normal (i-1) to (i)
671  // and normal (i) to (i+1) along axis i joint angle theta(i)
672  // signed angle between normal (i-1) to (i) and normal (i) to
673  // (i+1) along axis i
674  //
675  // First and last joints : a(0)= a(n) = 0
676  // alpha(0) = alpha(n) = 0
677  //
678  // PRISMATIC : theta(1) = 0 d(1) arbitrarily
679  //
680  // REVOLUTE : theta(1) arbitrarily d(1) = 0
681  //
682  // Not unique : if intersecting joint axis 2 choices for normal
683  // Frame assignment of the DH convention : Z(i-1) follows axis
684  // (i-1) X(i-1) is the normal between axis(i-1) and axis(i)
685  // Y(i-1) follows out of Z(i-1) and X(i-1)
686  //
687  // a(i-1) = distance from Z(i-1) to Z(i) along X(i-1)
688  // alpha(i-1) = angle between Z(i-1) to Z(i) along X(i-1)
689  // d(i) = distance from X(i-1) to X(i) along Z(i)
690  // theta(i) = angle between X(i-1) to X(i) along X(i)
691  // \endverbatim
692  */
693  static Frame DH_Craig1989(double a,double alpha,double d,double theta);
694 
695  // DH : constructs a transformationmatrix T_link(i-1)_link(i) with
696  // the Denavit-Hartenberg convention as described in the original
697  // publictation: Denavit, J. and Hartenberg, R. S., A kinematic
698  // notation for lower-pair mechanisms based on matrices, ASME
699  // Journal of Applied Mechanics, 23:215-221, 1955.
700 
701  static Frame DH(double a,double alpha,double d,double theta);
702 
703 
706  inline friend bool Equal(const Frame& a,const Frame& b,double eps);
707 
709  inline friend bool operator==(const Frame& a,const Frame& b);
711  inline friend bool operator!=(const Frame& a,const Frame& b);
712 };
713 
720 class Twist {
721 public:
724 public:
725 
727  Twist():vel(),rot() {};
728 
729  Twist(const Vector& _vel,const Vector& _rot):vel(_vel),rot(_rot) {};
730 
731  inline Twist& operator-=(const Twist& arg);
732  inline Twist& operator+=(const Twist& arg);
734  inline double& operator()(int i);
735 
738  inline double operator()(int i) const;
739 
740  double operator[] ( int index ) const
741  {
742  return this->operator() ( index );
743  }
744 
745  double& operator[] ( int index )
746  {
747  return this->operator() ( index );
748  }
749 
750  inline friend Twist operator*(const Twist& lhs,double rhs);
751  inline friend Twist operator*(double lhs,const Twist& rhs);
752  inline friend Twist operator/(const Twist& lhs,double rhs);
753  inline friend Twist operator+(const Twist& lhs,const Twist& rhs);
754  inline friend Twist operator-(const Twist& lhs,const Twist& rhs);
755  inline friend Twist operator-(const Twist& arg);
756  inline friend double dot(const Twist& lhs,const Wrench& rhs);
757  inline friend double dot(const Wrench& rhs,const Twist& lhs);
758  inline friend void SetToZero(Twist& v);
760  inline friend Twist operator*(const Twist& lhs,const Twist& rhs);
762  inline friend Wrench operator*(const Twist& lhs,const Wrench& rhs);
763 
765  static inline Twist Zero();
766 
768  inline void ReverseSign();
769 
776  inline Twist RefPoint(const Vector& v_base_AB) const;
777 
778 
781  inline friend bool Equal(const Twist& a,const Twist& b,double eps);
782 
784  inline friend bool operator==(const Twist& a,const Twist& b);
786  inline friend bool operator!=(const Twist& a,const Twist& b);
787 
788 // = Friends
789  friend class Rotation;
790  friend class Frame;
791 
792 };
793 
801 /*
802 class AccelerationTwist {
803 public:
804  Vector trans; //!< The translational acceleration of that point
805  Vector rot; //!< The rotational acceleration of that point.
806 public:
807 
809  AccelerationTwist():trans(),rot() {};
810 
811  AccelerationTwist(const Vector& _trans,const Vector& _rot):trans(_trans),rot(_rot) {};
812 
813  inline AccelerationTwist& operator-=(const AccelerationTwist& arg);
814  inline AccelerationTwist& operator+=(const AccelerationTwist& arg);
816  inline double& operator()(int i);
817 
820  inline double operator()(int i) const;
821 
822  double operator[] ( int index ) const
823  {
824  return this->operator() ( index );
825  }
826 
827  double& operator[] ( int index )
828  {
829  return this->operator() ( index );
830  }
831 
832  inline friend AccelerationTwist operator*(const AccelerationTwist& lhs,double rhs);
833  inline friend AccelerationTwist operator*(double lhs,const AccelerationTwist& rhs);
834  inline friend AccelerationTwist operator/(const AccelerationTwist& lhs,double rhs);
835  inline friend AccelerationTwist operator+(const AccelerationTwist& lhs,const AccelerationTwist& rhs);
836  inline friend AccelerationTwist operator-(const AccelerationTwist& lhs,const AccelerationTwist& rhs);
837  inline friend AccelerationTwist operator-(const AccelerationTwist& arg);
838  //inline friend double dot(const AccelerationTwist& lhs,const Wrench& rhs);
839  //inline friend double dot(const Wrench& rhs,const AccelerationTwist& lhs);
840  inline friend void SetToZero(AccelerationTwist& v);
841 
842 
844  static inline AccelerationTwist Zero();
845 
847  inline void ReverseSign();
848 
855  inline AccelerationTwist RefPoint(const Vector& v_base_AB) const;
856 
857 
860  inline friend bool Equal(const AccelerationTwist& a,const AccelerationTwist& b,double eps=epsilon);
861 
863  inline friend bool operator==(const AccelerationTwist& a,const AccelerationTwist& b);
865  inline friend bool operator!=(const AccelerationTwist& a,const AccelerationTwist& b);
866 
867 // = Friends
868  friend class Rotation;
869  friend class Frame;
870 
871 };
872 */
878 class Wrench
879 {
880 public:
883 public:
884 
886  Wrench():force(),torque() {};
887  Wrench(const Vector& _force,const Vector& _torque):force(_force),torque(_torque) {};
888 
889 // = Operators
890  inline Wrench& operator-=(const Wrench& arg);
891  inline Wrench& operator+=(const Wrench& arg);
892 
894  inline double& operator()(int i);
895 
898  inline double operator()(int i) const;
899 
900  double operator[] ( int index ) const
901  {
902  return this->operator() ( index );
903  }
904 
905  double& operator[] ( int index )
906  {
907  return this->operator() ( index );
908  }
909 
911  inline friend Wrench operator*(const Wrench& lhs,double rhs);
913  inline friend Wrench operator*(double lhs,const Wrench& rhs);
915  inline friend Wrench operator/(const Wrench& lhs,double rhs);
916 
917  inline friend Wrench operator+(const Wrench& lhs,const Wrench& rhs);
918  inline friend Wrench operator-(const Wrench& lhs,const Wrench& rhs);
919 
921  inline friend Wrench operator-(const Wrench& arg);
922 
925  inline friend void SetToZero(Wrench& v);
926 
928  static inline Wrench Zero();
929 
931  inline void ReverseSign();
932 
939  inline Wrench RefPoint(const Vector& v_base_AB) const;
940 
941 
944  inline friend bool Equal(const Wrench& a,const Wrench& b,double eps);
945 
947  inline friend bool operator==(const Wrench& a,const Wrench& b);
949  inline friend bool operator!=(const Wrench& a,const Wrench& b);
950 
951  friend class Rotation;
952  friend class Frame;
953 
954 
955 };
956 
957 
959 class Vector2
960 {
961  double data[2];
962 public:
964  Vector2() {data[0]=data[1] = 0.0;}
965  inline Vector2(double x,double y);
966  inline Vector2(const Vector2& arg);
967 
968  inline Vector2& operator = ( const Vector2& arg);
969 
971  inline double operator()(int index) const;
972 
974  inline double& operator() (int index);
975 
977  double operator[] ( int index ) const
978  {
979  return this->operator() ( index );
980  }
981 
983  double& operator[] ( int index )
984  {
985  return this->operator() ( index );
986  }
987 
988  inline double x() const;
989  inline double y() const;
990  inline void x(double);
991  inline void y(double);
992 
993  inline void ReverseSign();
994  inline Vector2& operator-=(const Vector2& arg);
995  inline Vector2& operator +=(const Vector2& arg);
996 
997 
998  inline friend Vector2 operator*(const Vector2& lhs,double rhs);
999  inline friend Vector2 operator*(double lhs,const Vector2& rhs);
1000  inline friend Vector2 operator/(const Vector2& lhs,double rhs);
1001  inline friend Vector2 operator+(const Vector2& lhs,const Vector2& rhs);
1002  inline friend Vector2 operator-(const Vector2& lhs,const Vector2& rhs);
1003  inline friend Vector2 operator*(const Vector2& lhs,const Vector2& rhs);
1004  inline friend Vector2 operator-(const Vector2& arg);
1005  inline friend void SetToZero(Vector2& v);
1006 
1008  inline static Vector2 Zero();
1009 
1015  double Normalize(double eps=epsilon);
1016 
1018  double Norm() const;
1019 
1021  inline void Set3DXY(const Vector& v);
1022 
1024  inline void Set3DYZ(const Vector& v);
1025 
1027  inline void Set3DZX(const Vector& v);
1028 
1032  inline void Set3DPlane(const Frame& F_someframe_XY,const Vector& v_someframe);
1033 
1034 
1037  inline friend bool Equal(const Vector2& a,const Vector2& b,double eps);
1038 
1040  inline friend bool operator==(const Vector2& a,const Vector2& b);
1042  inline friend bool operator!=(const Vector2& a,const Vector2& b);
1043 
1044  friend class Rotation2;
1045 };
1046 
1047 
1051 {
1052  double s,c;
1055 public:
1057  Rotation2() {c=1.0;s=0.0;}
1058 
1059  explicit Rotation2(double angle_rad):s(sin(angle_rad)),c(cos(angle_rad)) {}
1060 
1061  Rotation2(double ca,double sa):s(sa),c(ca){}
1062 
1063  inline Rotation2& operator=(const Rotation2& arg);
1064  inline Vector2 operator*(const Vector2& v) const;
1066  inline double operator() (int i,int j) const;
1067 
1068  inline friend Rotation2 operator *(const Rotation2& lhs,const Rotation2& rhs);
1069 
1070  inline void SetInverse();
1071  inline Rotation2 Inverse() const;
1072  inline Vector2 Inverse(const Vector2& v) const;
1073 
1074  inline void SetIdentity();
1075  inline static Rotation2 Identity();
1076 
1077 
1079  inline void SetRot(double angle);
1080 
1082  inline static Rotation2 Rot(double angle);
1083 
1085  inline double GetRot() const;
1086 
1089  inline friend bool Equal(const Rotation2& a,const Rotation2& b,double eps);
1090 };
1091 
1094 class Frame2
1095  {
1096 public:
1099 
1100 public:
1101 
1102  inline Frame2(const Rotation2& R,const Vector2& V);
1103  explicit inline Frame2(const Vector2& V);
1104  explicit inline Frame2(const Rotation2& R);
1105  inline Frame2(void);
1106  inline Frame2(const Frame2& arg);
1107  inline void Make4x4(double* d);
1108 
1111  inline double operator()(int i,int j);
1112 
1115  inline double operator() (int i,int j) const;
1116 
1117  inline void SetInverse();
1118  inline Frame2 Inverse() const;
1119  inline Vector2 Inverse(const Vector2& arg) const;
1120  inline Frame2& operator = (const Frame2& arg);
1121  inline Vector2 operator * (const Vector2& arg) const;
1122  inline friend Frame2 operator *(const Frame2& lhs,const Frame2& rhs);
1123  inline void SetIdentity();
1124  inline void Integrate(const Twist& t_this,double frequency);
1125  inline static Frame2 Identity() {
1126  Frame2 tmp;
1127  tmp.SetIdentity();
1128  return tmp;
1129  }
1130  inline friend bool Equal(const Frame2& a,const Frame2& b,double eps);
1131 };
1132 
1143 IMETHOD Vector diff(const Vector& p_w_a,const Vector& p_w_b,double dt=1);
1144 
1145 
1173 IMETHOD Vector diff(const Rotation& R_a_b1,const Rotation& R_a_b2,double dt=1);
1174 
1183 IMETHOD Twist diff(const Frame& F_a_b1,const Frame& F_a_b2,double dt=1);
1184 
1189 IMETHOD Twist diff(const Twist& a,const Twist& b,double dt=1);
1190 
1195 IMETHOD Wrench diff(const Wrench& W_a_p1,const Wrench& W_a_p2,double dt=1);
1196 
1204 IMETHOD Vector addDelta(const Vector& p_w_a,const Vector& p_w_da,double dt=1);
1205 
1218 IMETHOD Rotation addDelta(const Rotation& R_w_a,const Vector& da_w,double dt=1);
1219 
1230 IMETHOD Frame addDelta(const Frame& F_w_a,const Twist& da_w,double dt=1);
1231 
1239 IMETHOD Twist addDelta(const Twist& a,const Twist&da,double dt=1);
1240 
1241 
1250 IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt=1);
1251 
1252 #ifdef KDL_INLINE
1253 // #include "vector.inl"
1254 // #include "wrench.inl"
1255  //#include "rotation.inl"
1256  //#include "frame.inl"
1257  //#include "twist.inl"
1258  //#include "vector2.inl"
1259  //#include "rotation2.inl"
1260  //#include "frame2.inl"
1261 #include "frames.inl"
1262 #endif
1263 
1264 
1265 
1266 }
1267 
1268 
1269 #endif
KDL::Twist::Equal
friend bool Equal(const Twist &a, const Twist &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
KDL::Vector2::Normalize
double Normalize(double eps=epsilon)
Normalizes this vector and returns it norm makes v a unitvector and returns the norm of v.
Definition: frames.cpp:105
KDL::Wrench::operator-
friend Wrench operator-(const Wrench &arg)
An unary - operator.
KDL::Rotation::SetInverse
void SetInverse()
Sets the value of *this to its inverse.
Definition: frames.inl:648
KDL::Rotation::UnitY
void UnitY(const Vector &X)
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:527
KDL::Rotation::Rot2
static Rotation Rot2(const Vector &rotvec, double angle)
Along an arbitrary axes. rotvec should be normalized.
Definition: frames.cpp:304
KDL::Vector::data
double data[3]
Definition: frames.hpp:163
KDL::Wrench::operator+=
Wrench & operator+=(const Wrench &arg)
Definition: frames.inl:216
KDL::Rotation2::operator()
double operator()(int i, int j) const
Access to elements 0..1,0..1, bounds are checked when NDEBUG is not set.
Definition: frames.inl:853
frames.hpp
KDL::Frame2::Make4x4
void Make4x4(double *d)
KDL::Frame::Identity
static Frame Identity()
Definition: frames.inl:695
KDL::Wrench::operator+
friend Wrench operator+(const Wrench &lhs, const Wrench &rhs)
KDL::Vector::y
double y() const
Definition: frames.inl:76
KDL::Twist::rot
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:723
KDL::Frame2
A 2D frame class, for further documentation see the Frames class for methods with unchanged semantics...
Definition: frames.hpp:1095
KDL::Vector2::operator-
friend Vector2 operator-(const Vector2 &arg)
KDL::Frame::Inverse
Frame Inverse() const
Gives back inverse transformation of a Frame.
Definition: frames.inl:422
KDL::Vector::Set2DZX
void Set2DZX(const Vector2 &v)
a 3D vector where the 2D vector v is put in the ZX plane
Definition: frames.inl:475
KDL::Vector2::Set3DYZ
void Set3DYZ(const Vector &v)
projects v in its YZ plane, and sets *this to these values
Definition: frames.inl:820
KDL::operator*
ArticulatedBodyInertia operator*(double a, const ArticulatedBodyInertia &I)
Scalar product: I_new = double * I_old.
Definition: articulatedbodyinertia.cpp:51
frames.inl
KDL::Vector::operator*
friend Vector operator*(double lhs, const Vector &rhs)
Scalar multiplication is defined.
KDL::Rotation::GetRot
Vector GetRot() const
Returns a vector with the direction of the equiv.
Definition: frames.cpp:337
KDL::Rotation::Quaternion
static Rotation Quaternion(double x, double y, double z, double w)
Gives back a rotation matrix specified with Quaternion convention the norm of (x,y,...
Definition: frames.cpp:191
KDL::Frame2::Frame2
Frame2(void)
KDL::Frame::Equal
friend bool Equal(const Frame &a, const Frame &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
KDL::Rotation::Inverse
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
Definition: frames.inl:632
KDL::Rotation::operator!=
friend bool operator!=(const Rotation &a, const Rotation &b)
The literal inequality operator!=()
KDL::Frame::operator*
Vector operator*(const Vector &arg) const
Transformation of the base to which the vector is expressed.
Definition: frames.inl:412
KDL::Vector::Zero
static Vector Zero()
Definition: frames.inl:138
KDL::Vector2::operator-=
Vector2 & operator-=(const Vector2 &arg)
Definition: frames.inl:772
KDL::Frame::Frame
Frame()
Definition: frames.hpp:584
KDL::Frame::Integrate
void Integrate(const Twist &t_this, double frequency)
The twist <t_this> is expressed wrt the current frame.
Definition: frames.inl:619
KDL::Wrench::operator-
friend Wrench operator-(const Wrench &lhs, const Wrench &rhs)
KDL::Rotation::DoRotZ
void DoRotZ(double angle)
The DoRot...
Definition: frames.inl:583
KDL::Twist::dot
friend double dot(const Twist &lhs, const Wrench &rhs)
KDL::Rotation::operator()
double & operator()(int i, int j)
Access to elements 0..2,0..2, bounds are checked when NDEBUG is not set.
Definition: frames.inl:488
KDL::Twist::operator*
friend Twist operator*(const Twist &lhs, double rhs)
KDL::Rotation::GetEulerZYZ
void GetEulerZYZ(double &alpha, double &beta, double &gamma) const
Gives back the EulerZYZ convention description of the rotation matrix : First rotate around Z with al...
Definition: frames.cpp:276
KDL::Frame::p
Vector p
origine of the Frame
Definition: frames.hpp:572
KDL::Vector::operator*
friend Vector operator*(const Vector &lhs, const Vector &rhs)
KDL::Rotation::Rotation
Rotation()
Definition: frames.hpp:306
KDL::Vector2::operator[]
double operator[](int index) const
Equivalent to double operator()(int index) const.
Definition: frames.hpp:977
KDL::Vector2::operator=
Vector2 & operator=(const Vector2 &arg)
Definition: frames.inl:732
KDL::Twist::operator==
friend bool operator==(const Twist &a, const Twist &b)
The literal equality operator==(), also identical.
KDL::Twist::operator-
friend Twist operator-(const Twist &arg)
KDL::Rotation2::c
double c
Definition: frames.hpp:1052
KDL::Rotation::UnitX
void UnitX(const Vector &X)
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:515
KDL::Rotation::RotY
static Rotation RotY(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:605
KDL::Vector2::Equal
friend bool Equal(const Vector2 &a, const Vector2 &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
KDL::Twist::operator-=
Twist & operator-=(const Twist &arg)
Definition: frames.inl:312
KDL::Vector::operator==
friend bool operator==(const Vector &a, const Vector &b)
The literal equality operator==(), also identical.
KDL::Vector::operator!=
friend bool operator!=(const Vector &a, const Vector &b)
The literal inequality operator!=().
KDL::Rotation::DoRotX
void DoRotX(double angle)
The DoRot...
Definition: frames.inl:551
KDL::Wrench::operator/
friend Wrench operator/(const Wrench &lhs, double rhs)
Scalar division.
KDL::Rotation::GetRPY
void GetRPY(double &roll, double &pitch, double &yaw) const
Gives back a vector in RPY coordinates, variables are bound by.
Definition: frames.cpp:250
KDL::Vector2::operator/
friend Vector2 operator/(const Vector2 &lhs, double rhs)
KDL::Vector::Normalize
double Normalize(double eps=epsilon)
Normalizes this vector and returns it norm makes v a unitvector and returns the norm of v.
Definition: frames.cpp:148
KDL::Twist::operator*
friend Twist operator*(const Twist &lhs, const Twist &rhs)
Spatial cross product for 6d motion vectors, beware all of them have to be expressed in the same refe...
KDL::Wrench::operator[]
double operator[](int index) const
Definition: frames.hpp:900
KDL::Vector::Set2DYZ
void Set2DYZ(const Vector2 &v)
a 3D vector where the 2D vector v is put in the YZ plane
Definition: frames.inl:466
KDL::Twist::operator+
friend Twist operator+(const Twist &lhs, const Twist &rhs)
KDL::Vector::Equal
friend bool Equal(const Vector &a, const Vector &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
KDL::Vector::operator+
friend Vector operator+(const Vector &lhs, const Vector &rhs)
KDL::Frame2::SetInverse
void SetInverse()
Definition: frames.inl:944
KDL::Vector::operator+=
Vector & operator+=(const Vector &arg)
Adds a vector from the Vector object itself.
Definition: frames.inl:120
KDL::Rotation2::Rotation2
Rotation2()
c,s represent cos(angle), sin(angle), this also represents first col.
Definition: frames.hpp:1057
KDL::Frame::Make4x4
void Make4x4(double *d)
Reads data from an double array.
Definition: frames.cpp:40
KDL::addDelta
IMETHOD Vector addDelta(const Vector &p_w_a, const Vector &p_w_da, double dt=1)
adds vector da to vector a.
KDL
Definition: articulatedbodyinertia.cpp:28
KDL::Wrench::RefPoint
Wrench RefPoint(const Vector &v_base_AB) const
Changes the reference point of the wrench.
Definition: frames.inl:197
KDL::Vector2::Norm
double Norm() const
Definition: frames.cpp:88
KDL::Frame::operator==
friend bool operator==(const Frame &a, const Frame &b)
The literal equality operator==(), also identical.
KDL::Rotation2::s
double s
Definition: frames.hpp:1052
KDL::Vector::operator-
friend Vector operator-(const Vector &arg)
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:161
KDL::operator==
bool operator==(const Rotation &a, const Rotation &b)
Definition: frames.cpp:462
KDL::Wrench::operator()
double & operator()(int i)
index-based access to components, first force(0..2), then torque(3..5)
Definition: frames.inl:223
KDL::Frame2::operator*
Vector2 operator*(const Vector2 &arg) const
Definition: frames.inl:928
KDL::Vector::operator-=
Vector & operator-=(const Vector &arg)
subtracts a vector from the Vector object itself
Definition: frames.inl:129
KDL::Vector::Set2DPlane
void Set2DPlane(const Frame &F_someframe_XY, const Vector2 &v_XY)
a 3D vector where the 2D vector v_XY is put in the XY plane of the frame F_someframe_XY.
Definition: frames.inl:702
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:720
KDL::Rotation::DoRotY
void DoRotY(double angle)
The DoRot...
Definition: frames.inl:567
KDL::Twist::SetToZero
friend void SetToZero(Twist &v)
KDL::Rotation::GetEulerZYX
void GetEulerZYX(double &Alfa, double &Beta, double &Gamma) const
GetEulerZYX gets the euler ZYX parameters of a rotation : First rotate around Z with alfa,...
Definition: frames.hpp:493
KDL::Vector2::y
double y() const
Definition: frames.inl:795
KDL::Vector::SetToZero
friend void SetToZero(Vector &v)
To have a uniform operator to put an element to zero, for scalar values and for objects.
KDL::Vector::z
double z() const
Definition: frames.inl:77
KDL::Vector2::y
void y(double)
KDL::Rotation2::operator*
Vector2 operator*(const Vector2 &v) const
Definition: frames.inl:849
KDL::Vector::x
void x(double)
KDL::Wrench::torque
Vector torque
Torque that is applied at the origin of the current ref frame.
Definition: frames.hpp:882
KDL::Frame2::SetIdentity
void SetIdentity()
Definition: frames.inl:938
KDL::Frame2::Identity
static Frame2 Identity()
Definition: frames.hpp:1125
KDL::Frame2::operator=
Frame2 & operator=(const Frame2 &arg)
Definition: frames.inl:959
KDL::Rotation::GetQuaternion
void GetQuaternion(double &x, double &y, double &z, double &w) const
Get the quaternion of this matrix.
Definition: frames.cpp:205
KDL::Vector2::ReverseSign
void ReverseSign()
Definition: frames.inl:801
KDL::Rotation::RotZ
static Rotation RotZ(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:610
KDL::Rotation::Identity
static Rotation Identity()
Gives back an identity rotaton matrix.
Definition: frames.inl:547
KDL::Wrench
represents both translational and rotational acceleration.
Definition: frames.hpp:879
KDL::Rotation2::Inverse
Rotation2 Inverse() const
Definition: frames.inl:871
KDL::Rotation2::SetIdentity
void SetIdentity()
Definition: frames.inl:883
KDL::Vector2::Set3DZX
void Set3DZX(const Vector &v)
projects v in its ZX plane, and sets *this to these values
Definition: frames.inl:826
KDL::Frame::operator!=
friend bool operator!=(const Frame &a, const Frame &b)
The literal inequality operator!=().
KDL::Frame
Definition: frames.hpp:570
KDL::Twist::operator()
double & operator()(int i)
index-based access to components, first vel(0..2), then rot(3..5)
Definition: frames.inl:326
KDL::Wrench::operator!=
friend bool operator!=(const Wrench &a, const Wrench &b)
The literal inequality operator!=().
KDL::Twist::operator-
friend Twist operator-(const Twist &lhs, const Twist &rhs)
KDL::Vector::z
void z(double)
KDL::Wrench::operator*
friend Wrench operator*(double lhs, const Wrench &rhs)
Scalar multiplication.
KDL::Rotation::GetRotAngle
double GetRotAngle(Vector &axis, double eps=epsilon) const
Returns the rotation angle around the equiv.
Definition: frames.cpp:359
KDL::Rotation::operator*
Vector operator*(const Vector &v) const
Defines a multiplication R*V between a Rotation R and a Vector V.
Definition: frames.inl:521
KDL::Rotation::Equal
friend bool Equal(const Rotation &a, const Rotation &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
Definition: frames.cpp:160
KDL::Vector2::x
double x() const
Definition: frames.inl:794
KDL::Twist::operator/
friend Twist operator/(const Twist &lhs, double rhs)
KDL::Twist::ReverseSign
void ReverseSign()
Reverses the sign of the twist.
Definition: frames.inl:296
KDL::Vector::operator[]
double operator[](int index) const
Equivalent to double operator()(int index) const.
Definition: frames.hpp:183
KDL::Vector2::SetToZero
friend void SetToZero(Vector2 &v)
KDL::Frame2::Integrate
void Integrate(const Twist &t_this, double frequency)
KDL::Rotation2
A 2D Rotation class, for conventions see Rotation.
Definition: frames.hpp:1051
KDL::Rotation::EulerZYZ
static Rotation EulerZYZ(double Alfa, double Beta, double Gamma)
Gives back a rotation matrix specified with EulerZYZ convention :
Definition: frames.cpp:263
KDL::Frame::DH
static Frame DH(double a, double alpha, double d, double theta)
Definition: frames.cpp:71
KDL::Frame::DH_Craig1989
static Frame DH_Craig1989(double a, double alpha, double d, double theta)
Definition: frames.cpp:54
KDL::Vector::Set2DXY
void Set2DXY(const Vector2 &v)
a 3D vector where the 2D vector v is put in the XY plane
Definition: frames.inl:458
KDL::Wrench::operator==
friend bool operator==(const Wrench &a, const Wrench &b)
The literal equality operator==(), also identical.
KDL::Vector2::Set3DXY
void Set3DXY(const Vector &v)
projects v in its XY plane, and sets *this to these values
Definition: frames.inl:814
KDL::Twist::vel
Vector vel
The velocity of that point.
Definition: frames.hpp:722
KDL::Vector2::operator*
friend Vector2 operator*(const Vector2 &lhs, double rhs)
KDL::Frame2::operator()
double operator()(int i, int j)
Treats a frame as a 3x3 matrix and returns element i,j Access to elements 0..2,0.....
Definition: frames.inl:971
KDL::Rotation2::Rotation2
Rotation2(double angle_rad)
Definition: frames.hpp:1059
KDL::Frame2::p
Vector2 p
origine of the Frame
Definition: frames.hpp:1097
KDL::Twist::RefPoint
Twist RefPoint(const Vector &v_base_AB) const
Changes the reference point of the twist.
Definition: frames.inl:302
KDL::Rotation::data
double data[9]
Definition: frames.hpp:304
KDL::Twist::operator!=
friend bool operator!=(const Twist &a, const Twist &b)
The literal inequality operator!=().
KDL::Twist::Zero
static Twist Zero()
Definition: frames.inl:290
KDL::Wrench::operator*
friend Wrench operator*(const Wrench &lhs, double rhs)
Scalar multiplication.
KDL::Vector2::operator*
friend Vector2 operator*(const Vector2 &lhs, const Vector2 &rhs)
KDL::Vector2::operator==
friend bool operator==(const Vector2 &a, const Vector2 &b)
The literal equality operator==(), also identical.
KDL::Vector2::Set3DPlane
void Set3DPlane(const Frame &F_someframe_XY, const Vector &v_someframe)
projects v_someframe in the XY plane of F_someframe_XY, and sets *this to these values expressed wrt ...
Definition: frames.inl:833
KDL::Twist::dot
friend double dot(const Wrench &rhs, const Twist &lhs)
KDL::Rotation::EulerZYX
static Rotation EulerZYX(double Alfa, double Beta, double Gamma)
EulerZYX constructs a Rotation from the Euler ZYX parameters:
Definition: frames.hpp:469
KDL::Vector::Norm
double Norm() const
Definition: frames.cpp:118
KDL::Vector::operator-
friend Vector operator-(const Vector &lhs, const Vector &rhs)
KDL::Rotation::Rot
static Rotation Rot(const Vector &rotvec, double angle)
Along an arbitrary axes.
Definition: frames.cpp:294
KDL::Wrench::Zero
static Wrench Zero()
Definition: frames.inl:185
KDL::Rotation::operator==
friend bool operator==(const Rotation &a, const Rotation &b)
The literal equality operator==(), also identical.
Definition: frames.cpp:462
KDL::Equal
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
KDL::Twist::operator*
friend Twist operator*(double lhs, const Twist &rhs)
KDL::Vector2::Zero
static Vector2 Zero()
Definition: frames.inl:779
KDL::Rotation2::GetRot
double GetRot() const
Gets the angle (in radians)
Definition: frames.inl:897
KDL::Rotation::UnitY
Vector UnitY() const
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:522
KDL::Vector::operator*
friend Vector operator*(const Vector &lhs, double rhs)
Scalar multiplication is defined.
KDL::Wrench::SetToZero
friend void SetToZero(Wrench &v)
Sets the Wrench to Zero, to have a uniform function that sets an object or double to zero.
KDL::Rotation2::Rotation2
Rotation2(double ca, double sa)
Definition: frames.hpp:1061
KDL::Frame::M
Rotation M
Orientation of the Frame.
Definition: frames.hpp:573
KDL::Vector::ReverseSign
void ReverseSign()
Reverses the sign of the Vector object itself.
Definition: frames.inl:440
KDL::Twist::Twist
Twist()
The default constructor initialises to Zero via the constructor of Vector.
Definition: frames.hpp:727
KDL::Frame2::Inverse
Frame2 Inverse() const
Definition: frames.inl:952
KDL::Rotation2::operator=
Rotation2 & operator=(const Rotation2 &arg)
Definition: frames.inl:844
KDL::Rotation::operator=
Rotation & operator=(const Rotation &arg)
Definition: frames.inl:515
KDL::Rotation::RotX
static Rotation RotX(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:600
KDL::Vector::operator/
friend Vector operator/(const Vector &lhs, double rhs)
Scalar division is defined.
KDL::Vector2
2D version of Vector
Definition: frames.hpp:960
KDL::Vector::operator()
double operator()(int index) const
Access to elements, range checked when NDEBUG is not set, from 0..2.
Definition: frames.inl:143
KDL::Rotation::RPY
static Rotation RPY(double roll, double pitch, double yaw)
Gives back a rotation matrix specified with RPY convention: first rotate around X with roll,...
Definition: frames.cpp:238
KDL::Vector2::Vector2
Vector2()
Does not initialise to Zero().
Definition: frames.hpp:964
KDL::Frame::operator()
double operator()(int i, int j)
Treats a frame as a 4x4 matrix and returns element i,j Access to elements 0..3,0.....
Definition: frames.inl:662
KDL::Frame2::Equal
friend bool Equal(const Frame2 &a, const Frame2 &b, double eps)
KDL::Rotation2::SetRot
void SetRot(double angle)
The SetRot.. functions set the value of *this to the appropriate rotation matrix.
Definition: frames.inl:889
KDL::Vector2::operator!=
friend bool operator!=(const Vector2 &a, const Vector2 &b)
The literal inequality operator!=().
KDL::Vector2::operator+=
Vector2 & operator+=(const Vector2 &arg)
Definition: frames.inl:765
KDL::Rotation2::Identity
static Rotation2 Identity()
Definition: frames.inl:879
KDL::Vector::Vector
Vector()
Does not initialise the Vector to zero. use Vector::Zero() or SetToZero for that.
Definition: frames.hpp:165
KDL::Wrench::ReverseSign
void ReverseSign()
Reverses the sign of the current Wrench.
Definition: frames.inl:191
KDL::Vector2::data
double data[2]
Definition: frames.hpp:961
KDL::diff
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
determines the difference of vector b with vector a.
KDL::Wrench::Equal
friend bool Equal(const Wrench &a, const Wrench &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
KDL::Wrench::Wrench
Wrench(const Vector &_force, const Vector &_torque)
Definition: frames.hpp:887
KDL::Rotation2::SetInverse
void SetInverse()
Definition: frames.inl:867
KDL::Vector::dot
friend double dot(const Vector &lhs, const Vector &rhs)
KDL::Vector::x
double x() const
Definition: frames.inl:75
KDL::Vector::y
void y(double)
KDL::Wrench::force
Vector force
Force that is applied at the origin of the current ref frame.
Definition: frames.hpp:881
KDL::Vector2::operator()
double operator()(int index) const
Access to elements, range checked when NDEBUG is not set, from 0..1.
Definition: frames.inl:783
KDL::Vector2::operator+
friend Vector2 operator+(const Vector2 &lhs, const Vector2 &rhs)
KDL::Twist::operator[]
double operator[](int index) const
Definition: frames.hpp:740
KDL::Rotation2::Rot
static Rotation2 Rot(double angle)
The Rot... static functions give the value of the appropriate rotation matrix bac.
Definition: frames.inl:893
KDL::Rotation::UnitZ
void UnitZ(const Vector &X)
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:539
KDL::Rotation::UnitZ
Vector UnitZ() const
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:534
KDL::Twist::operator+=
Twist & operator+=(const Twist &arg)
Definition: frames.inl:319
KDL::Rotation::UnitX
Vector UnitX() const
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:510
KDL::Wrench::operator-=
Wrench & operator-=(const Wrench &arg)
Definition: frames.inl:209
KDL::Vector::operator=
Vector & operator=(const Vector &arg)
Assignment operator. The normal copy by value semantics.
Definition: frames.inl:49
KDL::Frame::operator=
Frame & operator=(const Frame &arg)
Normal copy-by-value semantics.
Definition: frames.inl:428
KDL::Wrench::Wrench
Wrench()
Does initialise force and torque to zero via the underlying constructor of Vector.
Definition: frames.hpp:886
KDL::Vector2::operator-
friend Vector2 operator-(const Vector2 &lhs, const Vector2 &rhs)
KDL::Twist::operator*
friend Wrench operator*(const Twist &lhs, const Wrench &rhs)
Spatial cross product for 6d force vectors, beware all of them have to be expressed in the same refer...
KDL::Frame2::M
Rotation2 M
Orientation of the Frame.
Definition: frames.hpp:1098
KDL::Vector2::operator*
friend Vector2 operator*(double lhs, const Vector2 &rhs)
KDL::Rotation2::Equal
friend bool Equal(const Rotation2 &a, const Rotation2 &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
KDL::Rotation
represents rotations in 3 dimensional space.
Definition: frames.hpp:302
KDL::Vector2::x
void x(double)
KDL::Twist::Twist
Twist(const Vector &_vel, const Vector &_rot)
Definition: frames.hpp:729