orocos_kdl
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 #include "utilities/hash_combine.h"
131 
132 #include <functional>
133 
135 
136 namespace KDL {
137 
138 
139 class Vector;
140 class Rotation;
141 class Frame;
142 class Wrench;
143 class Twist;
144 class Vector2;
145 class Rotation2;
146 class Frame2;
147 
148 
149 // Equal is friend function, but default arguments for friends are forbidden (ยง8.3.6.4)
150 inline bool Equal(const Vector& a,const Vector& b,double eps=epsilon);
151 inline bool Equal(const Frame& a,const Frame& b,double eps=epsilon);
152 inline bool Equal(const Twist& a,const Twist& b,double eps=epsilon);
153 inline bool Equal(const Wrench& a,const Wrench& b,double eps=epsilon);
154 inline bool Equal(const Vector2& a,const Vector2& b,double eps=epsilon);
155 inline bool Equal(const Rotation2& a,const Rotation2& b,double eps=epsilon);
156 inline bool Equal(const Frame2& a,const Frame2& b,double eps=epsilon);
157 
158 
162 class Vector
163 {
164 public:
165  double data[3];
167  inline Vector() {data[0]=data[1]=data[2] = 0.0;}
168 
170  inline Vector(double x,double y, double z);
171 
173  inline Vector(const Vector& arg);
174 
176  inline Vector& operator = ( const Vector& arg);
177 
179  inline double operator()(int index) const;
180 
182  inline double& operator() (int index);
183 
185  double operator[] ( int index ) const
186  {
187  return this->operator() ( index );
188  }
189 
191  double& operator[] ( int index )
192  {
193  return this->operator() ( index );
194  }
195 
196  inline double x() const;
197  inline double y() const;
198  inline double z() const;
199  inline void x(double);
200  inline void y(double);
201  inline void z(double);
202 
204  inline void ReverseSign();
205 
206 
208  inline Vector& operator-=(const Vector& arg);
209 
210 
212  inline Vector& operator +=(const Vector& arg);
213 
215  inline friend Vector operator*(const Vector& lhs,double rhs);
217  inline friend Vector operator*(double lhs,const Vector& rhs);
219 
220  inline friend Vector operator/(const Vector& lhs,double rhs);
221  inline friend Vector operator+(const Vector& lhs,const Vector& rhs);
222  inline friend Vector operator-(const Vector& lhs,const Vector& rhs);
223  inline friend Vector operator*(const Vector& lhs,const Vector& rhs);
224  inline friend Vector operator-(const Vector& arg);
225  inline friend double dot(const Vector& lhs,const Vector& rhs);
226 
229  inline friend void SetToZero(Vector& v);
230 
232  inline static Vector Zero();
233 
239  double Normalize(double eps=epsilon);
240 
242  double Norm(double eps=epsilon) const;
243 
244 
245 
247  inline void Set2DXY(const Vector2& v);
249  inline void Set2DYZ(const Vector2& v);
251  inline void Set2DZX(const Vector2& v);
253  inline void Set2DPlane(const Frame& F_someframe_XY,const Vector2& v_XY);
254 
255 
258  inline friend bool Equal(const Vector& a,const Vector& b,double eps);
259 
261  inline friend bool operator==(const Vector& a,const Vector& b);
263  inline friend bool operator!=(const Vector& a,const Vector& b);
264 
265  friend class Rotation;
266  friend class Frame;
267 };
268 
269 
303 class Rotation
304 {
305 public:
306  double data[9];
307 
308  inline Rotation() {
309  *this = Rotation::Identity();
310  }
311  inline Rotation(double Xx,double Yx,double Zx,
312  double Xy,double Yy,double Zy,
313  double Xz,double Yz,double Zz);
314  inline Rotation(const Vector& x,const Vector& y,const Vector& z);
315 
316  inline Rotation(const Rotation& arg);
317 
318  inline Rotation& operator=(const Rotation& arg);
319 
322  inline Vector operator*(const Vector& v) const;
323 
325  inline double& operator()(int i,int j);
326 
328  inline double operator() (int i,int j) const;
329 
330  friend Rotation operator *(const Rotation& lhs,const Rotation& rhs);
331 
333  inline void SetInverse();
334 
336  inline Rotation Inverse() const;
337 
339  inline Vector Inverse(const Vector& v) const;
340 
342  inline Wrench Inverse(const Wrench& arg) const;
343 
345  inline Twist Inverse(const Twist& arg) const;
346 
348  inline static Rotation Identity();
349 
350 
351 // = Rotations
353  inline static Rotation RotX(double angle);
355  inline static Rotation RotY(double angle);
357  inline static Rotation RotZ(double angle);
360  inline void DoRotX(double angle);
363  inline void DoRotY(double angle);
366  inline void DoRotZ(double angle);
367 
371  // @see Rot2 if you want to handle this error in another way.
372  static Rotation Rot(const Vector& rotvec,double angle);
373 
375  static Rotation Rot2(const Vector& rotvec,double angle);
376 
379  Vector GetRot() const;
380 
389  double GetRotAngle(Vector& axis,double eps=epsilon) const;
390 
391 
400  static Rotation EulerZYZ(double Alfa,double Beta,double Gamma);
401 
417  void GetEulerZYZ(double& alpha,double& beta,double& gamma) const;
418 
421  static Rotation Quaternion(double x,double y,double z, double w);
422 
425  void GetQuaternion(double& x,double& y,double& z, double& w) const;
426 
437  static Rotation RPY(double roll,double pitch,double yaw);
438 
457  void GetRPY(double& roll,double& pitch,double& yaw) const;
458 
459 
471  inline static Rotation EulerZYX(double Alfa,double Beta,double Gamma) {
472  return RPY(Gamma,Beta,Alfa);
473  }
474 
495  inline void GetEulerZYX(double& Alfa,double& Beta,double& Gamma) const {
496  GetRPY(Gamma,Beta,Alfa);
497  }
498 
503  inline Twist operator * (const Twist& arg) const;
504 
509  inline Wrench operator * (const Wrench& arg) const;
510 
512  inline Vector UnitX() const {
513  return Vector(data[0],data[3],data[6]);
514  }
515 
517  inline void UnitX(const Vector& X) {
518  data[0] = X(0);
519  data[3] = X(1);
520  data[6] = X(2);
521  }
522 
524  inline Vector UnitY() const {
525  return Vector(data[1],data[4],data[7]);
526  }
527 
529  inline void UnitY(const Vector& X) {
530  data[1] = X(0);
531  data[4] = X(1);
532  data[7] = X(2);
533  }
534 
536  inline Vector UnitZ() const {
537  return Vector(data[2],data[5],data[8]);
538  }
539 
541  inline void UnitZ(const Vector& X) {
542  data[2] = X(0);
543  data[5] = X(1);
544  data[8] = X(2);
545  }
546 
549  friend bool Equal(const Rotation& a,const Rotation& b,double eps);
550 
552  friend bool operator==(const Rotation& a,const Rotation& b);
554  friend bool operator!=(const Rotation& a,const Rotation& b);
555 
556  friend class Frame;
557 };
558  bool operator==(const Rotation& a,const Rotation& b);
559  bool Equal(const Rotation& a,const Rotation& b,double eps=epsilon);
560 
561 
562 
572 class Frame {
573 public:
576 
577 public:
578 
579  inline Frame(const Rotation& R,const Vector& V);
580 
582  explicit inline Frame(const Vector& V);
584  explicit inline Frame(const Rotation& R);
585 
586  inline Frame() {}
588  inline Frame(const Frame& arg);
589 
591  //\TODO should be formulated as a constructor
592  void Make4x4(double* d);
593 
596  inline double operator()(int i,int j);
597 
600  inline double operator() (int i,int j) const;
601 
602 // = Inverse
604  inline Frame Inverse() const;
605 
607  inline Vector Inverse(const Vector& arg) const;
608 
610  inline Wrench Inverse(const Wrench& arg) const;
611 
613  inline Twist Inverse(const Twist& arg) const;
614 
616  inline Frame& operator = (const Frame& arg);
617 
620  inline Vector operator * (const Vector& arg) const;
621 
628  inline Wrench operator * (const Wrench& arg) const;
629 
636  inline Twist operator * (const Twist& arg) const;
637 
639  inline friend Frame operator *(const Frame& lhs,const Frame& rhs);
640 
642  inline static Frame Identity();
643 
647  inline void Integrate(const Twist& t_this,double frequency);
648 
649  /*
650  // DH_Craig1989 : constructs a transformationmatrix
651  // T_link(i-1)_link(i) with the Denavit-Hartenberg convention as
652  // described in the Craigs book: Craig, J. J.,Introduction to
653  // Robotics: Mechanics and Control, Addison-Wesley,
654  // isbn:0-201-10326-5, 1986.
655  //
656  // Note that the frame is a redundant way to express the information
657  // in the DH-convention.
658  // \verbatim
659  // Parameters in full : a(i-1),alpha(i-1),d(i),theta(i)
660  //
661  // axis i-1 is connected by link i-1 to axis i numbering axis 1
662  // to axis n link 0 (immobile base) to link n
663  //
664  // link length a(i-1) length of the mutual perpendicular line
665  // (normal) between the 2 axes. This normal runs from (i-1) to
666  // (i) axis.
667  //
668  // link twist alpha(i-1): construct plane perpendicular to the
669  // normal project axis(i-1) and axis(i) into plane angle from
670  // (i-1) to (i) measured in the direction of the normal
671  //
672  // link offset d(i) signed distance between normal (i-1) to (i)
673  // and normal (i) to (i+1) along axis i joint angle theta(i)
674  // signed angle between normal (i-1) to (i) and normal (i) to
675  // (i+1) along axis i
676  //
677  // First and last joints : a(0)= a(n) = 0
678  // alpha(0) = alpha(n) = 0
679  //
680  // PRISMATIC : theta(1) = 0 d(1) arbitrarily
681  //
682  // REVOLUTE : theta(1) arbitrarily d(1) = 0
683  //
684  // Not unique : if intersecting joint axis 2 choices for normal
685  // Frame assignment of the DH convention : Z(i-1) follows axis
686  // (i-1) X(i-1) is the normal between axis(i-1) and axis(i)
687  // Y(i-1) follows out of Z(i-1) and X(i-1)
688  //
689  // a(i-1) = distance from Z(i-1) to Z(i) along X(i-1)
690  // alpha(i-1) = angle between Z(i-1) to Z(i) along X(i-1)
691  // d(i) = distance from X(i-1) to X(i) along Z(i)
692  // theta(i) = angle between X(i-1) to X(i) along X(i)
693  // \endverbatim
694  */
695  static Frame DH_Craig1989(double a,double alpha,double d,double theta);
696 
697  // DH : constructs a transformationmatrix T_link(i-1)_link(i) with
698  // the Denavit-Hartenberg convention as described in the original
699  // publictation: Denavit, J. and Hartenberg, R. S., A kinematic
700  // notation for lower-pair mechanisms based on matrices, ASME
701  // Journal of Applied Mechanics, 23:215-221, 1955.
702 
703  static Frame DH(double a,double alpha,double d,double theta);
704 
705 
708  inline friend bool Equal(const Frame& a,const Frame& b,double eps);
709 
711  inline friend bool operator==(const Frame& a,const Frame& b);
713  inline friend bool operator!=(const Frame& a,const Frame& b);
714 };
715 
716 
723 class Twist {
724 public:
727 public:
728 
730  Twist():vel(),rot() {};
731 
732  Twist(const Vector& _vel,const Vector& _rot):vel(_vel),rot(_rot) {};
733 
734  inline Twist& operator-=(const Twist& arg);
735  inline Twist& operator+=(const Twist& arg);
737  inline double& operator()(int i);
738 
741  inline double operator()(int i) const;
742 
743  double operator[] ( int index ) const
744  {
745  return this->operator() ( index );
746  }
747 
748  double& operator[] ( int index )
749  {
750  return this->operator() ( index );
751  }
752 
753  inline friend Twist operator*(const Twist& lhs,double rhs);
754  inline friend Twist operator*(double lhs,const Twist& rhs);
755  inline friend Twist operator/(const Twist& lhs,double rhs);
756  inline friend Twist operator+(const Twist& lhs,const Twist& rhs);
757  inline friend Twist operator-(const Twist& lhs,const Twist& rhs);
758  inline friend Twist operator-(const Twist& arg);
759  inline friend double dot(const Twist& lhs,const Wrench& rhs);
760  inline friend double dot(const Wrench& rhs,const Twist& lhs);
761  inline friend void SetToZero(Twist& v);
763  inline friend Twist operator*(const Twist& lhs,const Twist& rhs);
765  inline friend Wrench operator*(const Twist& lhs,const Wrench& rhs);
766 
768  static inline Twist Zero();
769 
771  inline void ReverseSign();
772 
779  inline Twist RefPoint(const Vector& v_base_AB) const;
780 
781 
784  inline friend bool Equal(const Twist& a,const Twist& b,double eps);
785 
787  inline friend bool operator==(const Twist& a,const Twist& b);
789  inline friend bool operator!=(const Twist& a,const Twist& b);
790 
791 // = Friends
792  friend class Rotation;
793  friend class Frame;
794 };
795 
796 
804 /*
805 class AccelerationTwist {
806 public:
807  Vector trans; //!< The translational acceleration of that point
808  Vector rot; //!< The rotational acceleration of that point.
809 public:
810 
812  AccelerationTwist():trans(),rot() {};
813 
814  AccelerationTwist(const Vector& _trans,const Vector& _rot):trans(_trans),rot(_rot) {};
815 
816  inline AccelerationTwist& operator-=(const AccelerationTwist& arg);
817  inline AccelerationTwist& operator+=(const AccelerationTwist& arg);
819  inline double& operator()(int i);
820 
823  inline double operator()(int i) const;
824 
825  double operator[] ( int index ) const
826  {
827  return this->operator() ( index );
828  }
829 
830  double& operator[] ( int index )
831  {
832  return this->operator() ( index );
833  }
834 
835  inline friend AccelerationTwist operator*(const AccelerationTwist& lhs,double rhs);
836  inline friend AccelerationTwist operator*(double lhs,const AccelerationTwist& rhs);
837  inline friend AccelerationTwist operator/(const AccelerationTwist& lhs,double rhs);
838  inline friend AccelerationTwist operator+(const AccelerationTwist& lhs,const AccelerationTwist& rhs);
839  inline friend AccelerationTwist operator-(const AccelerationTwist& lhs,const AccelerationTwist& rhs);
840  inline friend AccelerationTwist operator-(const AccelerationTwist& arg);
841  //inline friend double dot(const AccelerationTwist& lhs,const Wrench& rhs);
842  //inline friend double dot(const Wrench& rhs,const AccelerationTwist& lhs);
843  inline friend void SetToZero(AccelerationTwist& v);
844 
845 
847  static inline AccelerationTwist Zero();
848 
850  inline void ReverseSign();
851 
858  inline AccelerationTwist RefPoint(const Vector& v_base_AB) const;
859 
860 
863  inline friend bool Equal(const AccelerationTwist& a,const AccelerationTwist& b,double eps=epsilon);
864 
866  inline friend bool operator==(const AccelerationTwist& a,const AccelerationTwist& b);
868  inline friend bool operator!=(const AccelerationTwist& a,const AccelerationTwist& b);
869 
870 // = Friends
871  friend class Rotation;
872  friend class Frame;
873 
874 };
875 */
881 class Wrench
882 {
883 public:
886 public:
887 
889  Wrench():force(),torque() {};
890  Wrench(const Vector& _force,const Vector& _torque):force(_force),torque(_torque) {};
891 
892 // = Operators
893  inline Wrench& operator-=(const Wrench& arg);
894  inline Wrench& operator+=(const Wrench& arg);
895 
897  inline double& operator()(int i);
898 
901  inline double operator()(int i) const;
902 
903  double operator[] ( int index ) const
904  {
905  return this->operator() ( index );
906  }
907 
908  double& operator[] ( int index )
909  {
910  return this->operator() ( index );
911  }
912 
914  inline friend Wrench operator*(const Wrench& lhs,double rhs);
916  inline friend Wrench operator*(double lhs,const Wrench& rhs);
918  inline friend Wrench operator/(const Wrench& lhs,double rhs);
919 
920  inline friend Wrench operator+(const Wrench& lhs,const Wrench& rhs);
921  inline friend Wrench operator-(const Wrench& lhs,const Wrench& rhs);
922 
924  inline friend Wrench operator-(const Wrench& arg);
925 
928  inline friend void SetToZero(Wrench& v);
929 
931  static inline Wrench Zero();
932 
934  inline void ReverseSign();
935 
942  inline Wrench RefPoint(const Vector& v_base_AB) const;
943 
944 
947  inline friend bool Equal(const Wrench& a,const Wrench& b,double eps);
948 
950  inline friend bool operator==(const Wrench& a,const Wrench& b);
952  inline friend bool operator!=(const Wrench& a,const Wrench& b);
953 
954  friend class Rotation;
955  friend class Frame;
956 };
957 
958 
960 class Vector2
961 {
962  double data[2];
963 public:
965  Vector2() {data[0]=data[1] = 0.0;}
966  inline Vector2(double x,double y);
967  inline Vector2(const Vector2& arg);
968 
969  inline Vector2& operator = ( const Vector2& arg);
970 
972  inline double operator()(int index) const;
973 
975  inline double& operator() (int index);
976 
978  double operator[] ( int index ) const
979  {
980  return this->operator() ( index );
981  }
982 
984  double& operator[] ( int index )
985  {
986  return this->operator() ( index );
987  }
988 
989  inline double x() const;
990  inline double y() const;
991  inline void x(double);
992  inline void y(double);
993 
994  inline void ReverseSign();
995  inline Vector2& operator-=(const Vector2& arg);
996  inline Vector2& operator +=(const Vector2& arg);
997 
998 
999  inline friend Vector2 operator*(const Vector2& lhs,double rhs);
1000  inline friend Vector2 operator*(double lhs,const Vector2& rhs);
1001  inline friend Vector2 operator/(const Vector2& lhs,double 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& lhs,const Vector2& rhs);
1005  inline friend Vector2 operator-(const Vector2& arg);
1006  inline friend void SetToZero(Vector2& v);
1007 
1009  inline static Vector2 Zero();
1010 
1016  double Normalize(double eps=epsilon);
1017 
1019  double Norm(double eps=epsilon) const;
1020 
1022  inline void Set3DXY(const Vector& v);
1023 
1025  inline void Set3DYZ(const Vector& v);
1026 
1028  inline void Set3DZX(const Vector& v);
1029 
1033  inline void Set3DPlane(const Frame& F_someframe_XY,const Vector& v_someframe);
1034 
1035 
1038  inline friend bool Equal(const Vector2& a,const Vector2& b,double eps);
1039 
1041  inline friend bool operator==(const Vector2& a,const Vector2& b);
1043  inline friend bool operator!=(const Vector2& a,const Vector2& b);
1044 
1045  friend class Rotation2;
1046 };
1047 
1048 
1052 {
1053  double s,c;
1056 public:
1058  Rotation2() {c=1.0;s=0.0;}
1059 
1060  explicit Rotation2(double angle_rad):s(sin(angle_rad)),c(cos(angle_rad)) {}
1061 
1062  Rotation2(double ca,double sa):s(sa),c(ca){}
1063 
1064  Rotation2(const Rotation2& arg);
1065 
1066  inline Rotation2& operator=(const Rotation2& arg);
1067  inline Vector2 operator*(const Vector2& v) const;
1069  inline double operator() (int i,int j) const;
1070 
1071  inline friend Rotation2 operator *(const Rotation2& lhs,const Rotation2& rhs);
1072 
1073  inline void SetInverse();
1074  inline Rotation2 Inverse() const;
1075  inline Vector2 Inverse(const Vector2& v) const;
1076 
1077  inline void SetIdentity();
1078  inline static Rotation2 Identity();
1079 
1080 
1082  inline void SetRot(double angle);
1083 
1085  inline static Rotation2 Rot(double angle);
1086 
1088  inline double GetRot() const;
1089 
1092  inline friend bool Equal(const Rotation2& a,const Rotation2& b,double eps);
1093 };
1094 
1095 
1098 class Frame2
1099  {
1100 public:
1103 
1104 public:
1105 
1106  inline Frame2(const Rotation2& R,const Vector2& V);
1107  explicit inline Frame2(const Vector2& V);
1108  explicit inline Frame2(const Rotation2& R);
1109  inline Frame2(void);
1110  inline Frame2(const Frame2& arg);
1111  inline void Make4x4(double* d);
1112 
1115  inline double operator()(int i,int j);
1116 
1119  inline double operator() (int i,int j) const;
1120 
1121  inline void SetInverse();
1122  inline Frame2 Inverse() const;
1123  inline Vector2 Inverse(const Vector2& arg) const;
1124  inline Frame2& operator = (const Frame2& arg);
1125  inline Vector2 operator * (const Vector2& arg) const;
1126  inline friend Frame2 operator *(const Frame2& lhs,const Frame2& rhs);
1127  inline void SetIdentity();
1128  inline void Integrate(const Twist& t_this,double frequency);
1129  inline static Frame2 Identity() {
1130  Frame2 tmp;
1131  tmp.SetIdentity();
1132  return tmp;
1133  }
1134  inline friend bool Equal(const Frame2& a,const Frame2& b,double eps);
1135 };
1136 
1147 IMETHOD Vector diff(const Vector& p_w_a,const Vector& p_w_b,double dt=1);
1148 
1149 
1177 IMETHOD Vector diff(const Rotation& R_a_b1,const Rotation& R_a_b2,double dt=1);
1178 
1187 IMETHOD Twist diff(const Frame& F_a_b1,const Frame& F_a_b2,double dt=1);
1188 
1193 IMETHOD Twist diff(const Twist& a,const Twist& b,double dt=1);
1194 
1199 IMETHOD Wrench diff(const Wrench& W_a_p1,const Wrench& W_a_p2,double dt=1);
1200 
1208 IMETHOD Vector addDelta(const Vector& p_w_a,const Vector& p_w_da,double dt=1);
1209 
1222 IMETHOD Rotation addDelta(const Rotation& R_w_a,const Vector& da_w,double dt=1);
1223 
1234 IMETHOD Frame addDelta(const Frame& F_w_a,const Twist& da_w,double dt=1);
1235 
1243 IMETHOD Twist addDelta(const Twist& a,const Twist&da,double dt=1);
1244 
1245 
1254 IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt=1);
1255 
1256 #ifdef KDL_INLINE
1257 #include "frames.inl"
1258 #endif
1259 
1260 }
1261 
1262 template<> struct std::hash<KDL::Vector>
1263 {
1264  std::size_t operator()(KDL::Vector const& v) const noexcept
1265  {
1266  size_t seed = 0;
1267  KDL::hash_combine(seed, v.x());
1268  KDL::hash_combine(seed, v.y());
1269  KDL::hash_combine(seed, v.z());
1270  return seed;
1271  }
1272 };
1273 
1274 template<> struct std::hash<KDL::Rotation>
1275 {
1276  std::size_t operator()(KDL::Rotation const& r) const noexcept
1277  {
1278  size_t seed = 0;
1279  double x, y, z, w;
1280  r.GetQuaternion(x, y, z, w);
1281  KDL::hash_combine(seed, x);
1282  KDL::hash_combine(seed, y);
1283  KDL::hash_combine(seed, z);
1284  KDL::hash_combine(seed, w);
1285  return seed;
1286  }
1287 };
1288 
1289 template<> struct std::hash<KDL::Frame>
1290 {
1291  std::size_t operator()(KDL::Frame const& f) const noexcept
1292  {
1293  size_t seed = 0;
1294  KDL::hash_combine(seed, f.p);
1295  KDL::hash_combine(seed, f.M);
1296  return seed;
1297  }
1298 };
1299 
1300 template<> struct std::hash<KDL::Wrench>
1301 {
1302  std::size_t operator()(KDL::Wrench const& w) const noexcept
1303  {
1304  size_t seed = 0;
1305  KDL::hash_combine(seed, w.force);
1306  KDL::hash_combine(seed, w.torque);
1307  return seed;
1308  }
1309 };
1310 
1311 template<> struct std::hash<KDL::Twist>
1312 {
1313  std::size_t operator()(KDL::Twist const& t) const noexcept
1314  {
1315  size_t seed = 0;
1316  KDL::hash_combine(seed, t.vel);
1317  KDL::hash_combine(seed, t.rot);
1318  return seed;
1319  }
1320 };
1321 
1322 template<> struct std::hash<KDL::Vector2>
1323 {
1324  std::size_t operator()(KDL::Vector2 const& v) const noexcept
1325  {
1326  size_t seed = 0;
1327  KDL::hash_combine(seed, v.x());
1328  KDL::hash_combine(seed, v.y());
1329  return seed;
1330  }
1331 };
1332 
1333 template<> struct std::hash<KDL::Rotation2>
1334 {
1335  std::size_t operator()(KDL::Rotation2 const& r) const noexcept
1336  {
1337  size_t seed = 0;
1338  KDL::hash_combine(seed, r.GetRot());
1339  return seed;
1340  }
1341 };
1342 
1343 template<> struct std::hash<KDL::Frame2>
1344 {
1345  std::size_t operator()(KDL::Frame2 const& f) const noexcept
1346  {
1347  size_t seed = 0;
1348  KDL::hash_combine(seed, f.p);
1349  KDL::hash_combine(seed, f.M);
1350  return seed;
1351  }
1352 };
1353 
1354 #endif
KDL::Twist::Equal
friend bool Equal(const Twist &a, const Twist &b, double eps)
KDL::Vector2::Normalize
double Normalize(double eps=epsilon)
Definition: frames.cpp:129
KDL::Rotation::SetInverse
void SetInverse()
Sets the value of *this to its inverse.
Definition: frames.inl:653
KDL::Rotation::UnitY
void UnitY(const Vector &X)
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:529
KDL::Rotation::Rot2
static Rotation Rot2(const Vector &rotvec, double angle)
Along an arbitrary axes. rotvec should be normalized.
Definition: frames.cpp:328
KDL::Vector::data
double data[3]
Definition: frames.hpp:165
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:860
KDL::Frame2::Make4x4
void Make4x4(double *d)
KDL::Frame::Identity
static Frame Identity()
Definition: frames.inl:700
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:726
KDL::Frame2
Definition: frames.hpp:1098
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:825
frames.inl
KDL::Rotation::GetRot
Vector GetRot() const
Definition: frames.cpp:361
functional
KDL::Rotation::Quaternion
static Rotation Quaternion(double x, double y, double z, double w)
Definition: frames.cpp:215
KDL::Frame2::Frame2
Frame2(void)
KDL::Frame::Equal
friend bool Equal(const Frame &a, const Frame &b, double eps)
KDL::Rotation::Inverse
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
Definition: frames.inl:637
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
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:777
KDL::Frame::Frame
Frame()
Definition: frames.hpp:586
KDL::Frame::Integrate
void Integrate(const Twist &t_this, double frequency)
Definition: frames.inl:624
KDL::Wrench::operator-
friend Wrench operator-(const Wrench &lhs, const Wrench &rhs)
KDL::Rotation::DoRotZ
void DoRotZ(double angle)
Definition: frames.inl:588
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
Definition: frames.cpp:300
KDL::Frame::p
Vector p
origine of the Frame
Definition: frames.hpp:574
KDL::Rotation::Rotation
Rotation()
Definition: frames.hpp:308
KDL::Vector2::operator[]
double operator[](int index) const
Equivalent to double operator()(int index) const.
Definition: frames.hpp:978
KDL::Vector2::operator=
Vector2 & operator=(const Vector2 &arg)
Definition: frames.inl:737
KDL::Twist::operator==
friend bool operator==(const Twist &a, const Twist &b)
The literal equality operator==(), also identical.
KDL::Rotation2::c
double c
Definition: frames.hpp:1053
KDL::Rotation::UnitX
void UnitX(const Vector &X)
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:517
KDL::Vector::Norm
double Norm(double eps=epsilon) const
Definition: frames.cpp:142
KDL::Rotation::RotY
static Rotation RotY(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:610
KDL::Vector2::Equal
friend bool Equal(const Vector2 &a, const Vector2 &b, double eps)
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)
Definition: frames.inl:556
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
Definition: frames.cpp:274
utility.h
KDL::sin
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:335
KDL::Vector2::operator/
friend Vector2 operator/(const Vector2 &lhs, double rhs)
KDL::hash_combine
void hash_combine(std::size_t &seed, const T &v)
Combine hash of object v to the seed.
Definition: hash_combine.h:18
KDL::Vector::Normalize
double Normalize(double eps=epsilon)
Definition: frames.cpp:172
KDL::Wrench::operator[]
double operator[](int index) const
Definition: frames.hpp:903
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)
KDL::Vector::operator+
friend Vector operator+(const Vector &lhs, const Vector &rhs)
KDL::Frame2::SetInverse
void SetInverse()
Definition: frames.inl:951
KDL::Vector::operator+=
Vector & operator+=(const Vector &arg)
Adds a vector from the Vector object itself.
Definition: frames.inl:120
KDL::Rotation2::Rotation2
Rotation2()
Default constructor does NOT initialise to Zero().
Definition: frames.hpp:1058
KDL::Frame::Make4x4
void Make4x4(double *d)
Reads data from an double array.
Definition: frames.cpp:64
KDL::addDelta
IMETHOD Vector addDelta(const Vector &p_w_a, const Vector &p_w_da, double dt=1)
adds vector da to vector a. see also the corresponding diff() routine.
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::Wrench::RefPoint
Wrench RefPoint(const Vector &v_base_AB) const
Definition: frames.inl:197
KDL::Frame::operator==
friend bool operator==(const Frame &a, const Frame &b)
The literal equality operator==(), also identical.
std::hash< KDL::Frame2 >::operator()
std::size_t operator()(KDL::Frame2 const &f) const noexcept
Definition: frames.hpp:1345
KDL::Rotation2::s
double s
Definition: frames.hpp:1053
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:162
KDL::operator==
bool operator==(const Rotation &a, const Rotation &b)
Definition: frames.cpp:455
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:935
KDL::cos
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:343
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:707
KDL::epsilon
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
Definition: utility.cxx:21
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:723
KDL::Rotation::DoRotY
void DoRotY(double angle)
Definition: frames.inl:572
KDL::Twist::SetToZero
friend void SetToZero(Twist &v)
KDL::Rotation::GetEulerZYX
void GetEulerZYX(double &Alfa, double &Beta, double &Gamma) const
Definition: frames.hpp:495
KDL::Vector2::y
double y() const
Definition: frames.inl:800
KDL::Vector::SetToZero
friend void SetToZero(Vector &v)
KDL::Vector::z
double z() const
Definition: frames.inl:77
KDL::Rotation2::operator*
Vector2 operator*(const Vector2 &v) const
Definition: frames.inl:856
KDL::Wrench::torque
Vector torque
Torque that is applied at the origin of the current ref frame.
Definition: frames.hpp:885
KDL::Frame2::SetIdentity
void SetIdentity()
Definition: frames.inl:945
KDL::Frame2::Identity
static Frame2 Identity()
Definition: frames.hpp:1129
KDL::Frame2::operator=
Frame2 & operator=(const Frame2 &arg)
Definition: frames.inl:966
KDL::Rotation::GetQuaternion
void GetQuaternion(double &x, double &y, double &z, double &w) const
Definition: frames.cpp:229
KDL::Vector2::ReverseSign
void ReverseSign()
Definition: frames.inl:806
KDL::Rotation::RotZ
static Rotation RotZ(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:615
KDL::Rotation::Identity
static Rotation Identity()
Gives back an identity rotaton matrix.
Definition: frames.inl:552
KDL::Wrench
represents both translational and rotational acceleration.
Definition: frames.hpp:881
KDL::Rotation2::Inverse
Rotation2 Inverse() const
Definition: frames.inl:878
KDL::Rotation2::SetIdentity
void SetIdentity()
Definition: frames.inl:890
KDL::Vector2::Set3DZX
void Set3DZX(const Vector &v)
projects v in its ZX plane, and sets *this to these values
Definition: frames.inl:831
KDL::Frame::operator!=
friend bool operator!=(const Frame &a, const Frame &b)
The literal inequality operator!=().
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
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::Rotation::GetRotAngle
double GetRotAngle(Vector &axis, double eps=epsilon) const
Definition: frames.cpp:383
KDL::Rotation::operator*
Vector operator*(const Vector &v) const
Definition: frames.inl:526
KDL::Rotation::Equal
friend bool Equal(const Rotation &a, const Rotation &b, double eps)
Definition: frames.cpp:184
KDL::Vector2::x
double x() const
Definition: frames.inl:799
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:185
KDL::Vector2::SetToZero
friend void SetToZero(Vector2 &v)
KDL::Frame2::Integrate
void Integrate(const Twist &t_this, double frequency)
std::hash< KDL::Frame >::operator()
std::size_t operator()(KDL::Frame const &f) const noexcept
Definition: frames.hpp:1291
std::hash< KDL::Vector >::operator()
std::size_t operator()(KDL::Vector const &v) const noexcept
Definition: frames.hpp:1264
KDL::Rotation2
Definition: frames.hpp:1051
KDL::Rotation::EulerZYZ
static Rotation EulerZYZ(double Alfa, double Beta, double Gamma)
Definition: frames.cpp:287
KDL::Frame::DH
static Frame DH(double a, double alpha, double d, double theta)
Definition: frames.cpp:95
KDL::Frame::DH_Craig1989
static Frame DH_Craig1989(double a, double alpha, double d, double theta)
Definition: frames.cpp:78
std::hash< KDL::Twist >::operator()
std::size_t operator()(KDL::Twist const &t) const noexcept
Definition: frames.hpp:1313
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:819
KDL::Twist::vel
Vector vel
The velocity of that point.
Definition: frames.hpp:725
KDL::Vector2::operator*
friend Vector2 operator*(const Vector2 &lhs, double rhs)
KDL::Frame2::operator()
double operator()(int i, int j)
Definition: frames.inl:978
KDL::Rotation2::Rotation2
Rotation2(double angle_rad)
Definition: frames.hpp:1060
KDL::Frame2::p
Vector2 p
origine of the Frame
Definition: frames.hpp:1101
KDL::Twist::RefPoint
Twist RefPoint(const Vector &v_base_AB) const
Definition: frames.inl:302
KDL::Rotation::data
double data[9]
Definition: frames.hpp:306
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 bool operator==(const Vector2 &a, const Vector2 &b)
The literal equality operator==(), also identical.
std::hash< KDL::Wrench >::operator()
std::size_t operator()(KDL::Wrench const &w) const noexcept
Definition: frames.hpp:1302
KDL::Vector2::Set3DPlane
void Set3DPlane(const Frame &F_someframe_XY, const Vector &v_someframe)
Definition: frames.inl:838
KDL::Rotation::EulerZYX
static Rotation EulerZYX(double Alfa, double Beta, double Gamma)
Definition: frames.hpp:471
KDL::Vector::operator-
friend Vector operator-(const Vector &lhs, const Vector &rhs)
KDL::Rotation::Rot
static Rotation Rot(const Vector &rotvec, double angle)
Definition: frames.cpp:318
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:455
KDL::Equal
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
KDL::Vector2::Zero
static Vector2 Zero()
Definition: frames.inl:784
KDL::Rotation2::GetRot
double GetRot() const
Gets the angle (in radians)
Definition: frames.inl:904
KDL::Rotation::UnitY
Vector UnitY() const
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:524
KDL::Vector::operator*
friend Vector operator*(const Vector &lhs, double rhs)
Scalar multiplication is defined.
KDL::Wrench::SetToZero
friend void SetToZero(Wrench &v)
KDL::Rotation2::Rotation2
Rotation2(double ca, double sa)
Definition: frames.hpp:1062
KDL::Frame::M
Rotation M
Orientation of the Frame.
Definition: frames.hpp:575
std::hash< KDL::Vector2 >::operator()
std::size_t operator()(KDL::Vector2 const &v) const noexcept
Definition: frames.hpp:1324
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:730
KDL::Frame2::Inverse
Frame2 Inverse() const
Definition: frames.inl:959
KDL::Rotation2::operator=
Rotation2 & operator=(const Rotation2 &arg)
Definition: frames.inl:851
KDL::Rotation::operator=
Rotation & operator=(const Rotation &arg)
Definition: frames.inl:520
KDL::Rotation::RotX
static Rotation RotX(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:605
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
std::hash< KDL::Rotation >::operator()
std::size_t operator()(KDL::Rotation const &r) const noexcept
Definition: frames.hpp:1276
KDL::Rotation::RPY
static Rotation RPY(double roll, double pitch, double yaw)
Definition: frames.cpp:262
KDL::Vector2::Vector2
Vector2()
Does not initialise to Zero().
Definition: frames.hpp:965
KDL::Vector2::Norm
double Norm(double eps=epsilon) const
Definition: frames.cpp:112
std::size_t
KDL::Frame::operator()
double operator()(int i, int j)
Definition: frames.inl:667
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:896
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:770
KDL::Rotation2::Identity
static Rotation2 Identity()
Definition: frames.inl:886
KDL::Vector::Vector
Vector()
Does not initialise the Vector to zero. use Vector::Zero() or SetToZero for that.
Definition: frames.hpp:167
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:962
KDL::diff
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
KDL::Wrench::Equal
friend bool Equal(const Wrench &a, const Wrench &b, double eps)
KDL::Wrench::Wrench
Wrench(const Vector &_force, const Vector &_torque)
Definition: frames.hpp:890
std::hash< KDL::Rotation2 >::operator()
std::size_t operator()(KDL::Rotation2 const &r) const noexcept
Definition: frames.hpp:1335
KDL::Rotation2::SetInverse
void SetInverse()
Definition: frames.inl:874
KDL::Vector::dot
friend double dot(const Vector &lhs, const Vector &rhs)
KDL::Vector::x
double x() const
Definition: frames.inl:75
KDL::Wrench::force
Vector force
Force that is applied at the origin of the current ref frame.
Definition: frames.hpp:884
KDL::Vector2::operator()
double operator()(int index) const
Access to elements, range checked when NDEBUG is not set, from 0..1.
Definition: frames.inl:788
KDL::Vector2::operator+
friend Vector2 operator+(const Vector2 &lhs, const Vector2 &rhs)
IMETHOD
#define IMETHOD
Definition: utility.h:41
KDL::Twist::operator[]
double operator[](int index) const
Definition: frames.hpp:743
KDL::Rotation2::Rot
static Rotation2 Rot(double angle)
The Rot... static functions give the value of the appropriate rotation matrix bac.
Definition: frames.inl:900
KDL::Rotation::UnitZ
void UnitZ(const Vector &X)
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:541
KDL::Rotation::UnitZ
Vector UnitZ() const
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:536
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:512
kdl-config.h
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
hash_combine.h
KDL::Wrench::Wrench
Wrench()
Does initialise force and torque to zero via the underlying constructor of Vector.
Definition: frames.hpp:889
KDL::Vector2::operator-
friend Vector2 operator-(const Vector2 &lhs, const Vector2 &rhs)
KDL::Frame2::M
Rotation2 M
Orientation of the Frame.
Definition: frames.hpp:1102
std::hash
KDL::Rotation2::Equal
friend bool Equal(const Rotation2 &a, const Rotation2 &b, double eps)
KDL::Rotation
represents rotations in 3 dimensional space.
Definition: frames.hpp:303
KDL::Twist::Twist
Twist(const Vector &_vel, const Vector &_rot)
Definition: frames.hpp:732