KDL 1.5.1
Loading...
Searching...
No Matches
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
133namespace KDL {
134
135
136
137class Vector;
138class Rotation;
139class Frame;
140class Wrench;
141class Twist;
142class Vector2;
143class Rotation2;
144class Frame2;
145
146
147// Equal is friend function, but default arguments for friends are forbidden (ยง8.3.6.4)
148inline bool Equal(const Vector& a,const Vector& b,double eps=epsilon);
149inline bool Equal(const Frame& a,const Frame& b,double eps=epsilon);
150inline bool Equal(const Twist& a,const Twist& b,double eps=epsilon);
151inline bool Equal(const Wrench& a,const Wrench& b,double eps=epsilon);
152inline bool Equal(const Vector2& a,const Vector2& b,double eps=epsilon);
153inline bool Equal(const Rotation2& a,const Rotation2& b,double eps=epsilon);
154inline bool Equal(const Frame2& a,const Frame2& b,double eps=epsilon);
155
156
161{
162public:
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(double eps=epsilon) 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
302{
303public:
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
314 inline Rotation(const Rotation& arg);
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
570class Frame {
571public:
574
575public:
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
720class Twist {
721public:
724public:
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/*
802class AccelerationTwist {
803public:
804 Vector trans; //!< The translational acceleration of that point
805 Vector rot; //!< The rotational acceleration of that point.
806public:
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*/
879{
880public:
883public:
884
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
960{
961 double data[2];
962public:
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(double eps=epsilon) 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;
1055public:
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 Rotation2(const Rotation2& arg);
1064
1065 inline Rotation2& operator=(const Rotation2& arg);
1066 inline Vector2 operator*(const Vector2& v) const;
1068 inline double operator() (int i,int j) const;
1069
1070 inline friend Rotation2 operator *(const Rotation2& lhs,const Rotation2& rhs);
1071
1072 inline void SetInverse();
1073 inline Rotation2 Inverse() const;
1074 inline Vector2 Inverse(const Vector2& v) const;
1075
1076 inline void SetIdentity();
1077 inline static Rotation2 Identity();
1078
1079
1081 inline void SetRot(double angle);
1082
1084 inline static Rotation2 Rot(double angle);
1085
1087 inline double GetRot() const;
1088
1091 inline friend bool Equal(const Rotation2& a,const Rotation2& b,double eps);
1092};
1093
1097 {
1098public:
1101
1102public:
1103
1104 inline Frame2(const Rotation2& R,const Vector2& V);
1105 explicit inline Frame2(const Vector2& V);
1106 explicit inline Frame2(const Rotation2& R);
1107 inline Frame2(void);
1108 inline Frame2(const Frame2& arg);
1109 inline void Make4x4(double* d);
1110
1113 inline double operator()(int i,int j);
1114
1117 inline double operator() (int i,int j) const;
1118
1119 inline void SetInverse();
1120 inline Frame2 Inverse() const;
1121 inline Vector2 Inverse(const Vector2& arg) const;
1122 inline Frame2& operator = (const Frame2& arg);
1123 inline Vector2 operator * (const Vector2& arg) const;
1124 inline friend Frame2 operator *(const Frame2& lhs,const Frame2& rhs);
1125 inline void SetIdentity();
1126 inline void Integrate(const Twist& t_this,double frequency);
1127 inline static Frame2 Identity() {
1128 Frame2 tmp;
1129 tmp.SetIdentity();
1130 return tmp;
1131 }
1132 inline friend bool Equal(const Frame2& a,const Frame2& b,double eps);
1133};
1134
1145IMETHOD Vector diff(const Vector& p_w_a,const Vector& p_w_b,double dt=1);
1146
1147
1175IMETHOD Vector diff(const Rotation& R_a_b1,const Rotation& R_a_b2,double dt=1);
1176
1185IMETHOD Twist diff(const Frame& F_a_b1,const Frame& F_a_b2,double dt=1);
1186
1191IMETHOD Twist diff(const Twist& a,const Twist& b,double dt=1);
1192
1197IMETHOD Wrench diff(const Wrench& W_a_p1,const Wrench& W_a_p2,double dt=1);
1198
1206IMETHOD Vector addDelta(const Vector& p_w_a,const Vector& p_w_da,double dt=1);
1207
1220IMETHOD Rotation addDelta(const Rotation& R_w_a,const Vector& da_w,double dt=1);
1221
1232IMETHOD Frame addDelta(const Frame& F_w_a,const Twist& da_w,double dt=1);
1233
1241IMETHOD Twist addDelta(const Twist& a,const Twist&da,double dt=1);
1242
1243
1252IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt=1);
1253
1254#ifdef KDL_INLINE
1255#include "frames.inl"
1256#endif
1257
1258}
1259
1260
1261#endif
A 2D frame class, for further documentation see the Frames class for methods with unchanged semantics...
Definition: frames.hpp:1097
void SetIdentity()
Definition: frames.inl:945
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:978
friend Frame2 operator*(const Frame2 &lhs, const Frame2 &rhs)
static Frame2 Identity()
Definition: frames.hpp:1127
Frame2 & operator=(const Frame2 &arg)
Definition: frames.inl:966
void Integrate(const Twist &t_this, double frequency)
Rotation2 M
Orientation of the Frame.
Definition: frames.hpp:1100
friend bool Equal(const Frame2 &a, const Frame2 &b, double eps)
Vector2 p
origine of the Frame
Definition: frames.hpp:1099
void SetInverse()
Definition: frames.inl:951
void Make4x4(double *d)
Frame2 Inverse() const
Definition: frames.inl:959
Definition: frames.hpp:570
static Frame DH_Craig1989(double a, double alpha, double d, double theta)
Definition: frames.cpp:53
static Frame Identity()
Definition: frames.inl:700
Rotation M
Orientation of the Frame.
Definition: frames.hpp:573
void Make4x4(double *d)
Reads data from an double array.
Definition: frames.cpp:39
void Integrate(const Twist &t_this, double frequency)
The twist <t_this> is expressed wrt the current frame.
Definition: frames.inl:624
Frame & operator=(const Frame &arg)
Normal copy-by-value semantics.
Definition: frames.inl:428
static Frame DH(double a, double alpha, double d, double theta)
Definition: frames.cpp:70
friend bool operator==(const Frame &a, const Frame &b)
The literal equality operator==(), also identical.
friend bool Equal(const Frame &a, const Frame &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
Vector p
origine of the Frame
Definition: frames.hpp:572
Frame Inverse() const
Gives back inverse transformation of a Frame.
Definition: frames.inl:422
friend Frame operator*(const Frame &lhs, const Frame &rhs)
Composition of two frames.
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:667
Frame()
Definition: frames.hpp:584
friend bool operator!=(const Frame &a, const Frame &b)
The literal inequality operator!=().
A 2D Rotation class, for conventions see Rotation.
Definition: frames.hpp:1051
double GetRot() const
Gets the angle (in radians)
Definition: frames.inl:904
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
Rotation2(double ca, double sa)
Definition: frames.hpp:1061
static Rotation2 Rot(double angle)
The Rot... static functions give the value of the appropriate rotation matrix bac.
Definition: frames.inl:900
void SetInverse()
Definition: frames.inl:874
Rotation2()
c,s represent cos(angle), sin(angle), this also represents first col.
Definition: frames.hpp:1057
static Rotation2 Identity()
Definition: frames.inl:886
friend bool Equal(const Rotation2 &a, const Rotation2 &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
friend Rotation2 operator*(const Rotation2 &lhs, const Rotation2 &rhs)
double s
Definition: frames.hpp:1052
double c
Definition: frames.hpp:1052
void SetIdentity()
Definition: frames.inl:890
Rotation2(double angle_rad)
Definition: frames.hpp:1059
Rotation2 Inverse() const
Definition: frames.inl:878
void SetRot(double angle)
The SetRot.. functions set the value of *this to the appropriate rotation matrix.
Definition: frames.inl:896
Rotation2 & operator=(const Rotation2 &arg)
Definition: frames.inl:851
represents rotations in 3 dimensional space.
Definition: frames.hpp:302
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:275
Vector GetRot() const
Returns a vector with the direction of the equiv.
Definition: frames.cpp:336
void GetRPY(double &roll, double &pitch, double &yaw) const
Gives back a vector in RPY coordinates, variables are bound by.
Definition: frames.cpp:249
void DoRotZ(double angle)
The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot.
Definition: frames.inl:588
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
Definition: frames.inl:637
Rotation & operator=(const Rotation &arg)
Definition: frames.inl:520
Vector UnitY() const
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:522
void UnitZ(const Vector &X)
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:539
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:190
Vector UnitZ() const
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:534
Vector UnitX() const
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:510
Rotation()
Definition: frames.hpp:306
double GetRotAngle(Vector &axis, double eps=epsilon) const
Returns the rotation angle around the equiv.
Definition: frames.cpp:358
static Rotation EulerZYX(double Alfa, double Beta, double Gamma)
EulerZYX constructs a Rotation from the Euler ZYX parameters:
Definition: frames.hpp:469
static Rotation Rot2(const Vector &rotvec, double angle)
Along an arbitrary axes. rotvec should be normalized.
Definition: frames.cpp:303
friend Rotation operator*(const Rotation &lhs, const Rotation &rhs)
Definition: frames.cpp:173
static Rotation Identity()
Gives back an identity rotaton matrix.
Definition: frames.inl:552
static Rotation RotX(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:605
friend bool operator==(const Rotation &a, const Rotation &b)
The literal equality operator==(), also identical.
Definition: frames.cpp:430
friend bool operator!=(const Rotation &a, const Rotation &b)
The literal inequality operator!=()
void UnitX(const Vector &X)
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:515
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:159
void DoRotX(double angle)
The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot.
Definition: frames.inl:556
double data[9]
Definition: frames.hpp:304
static Rotation RotY(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:610
void GetQuaternion(double &x, double &y, double &z, double &w) const
Get the quaternion of this matrix.
Definition: frames.cpp:204
void SetInverse()
Sets the value of *this to its inverse.
Definition: frames.inl:653
static Rotation Rot(const Vector &rotvec, double angle)
Along an arbitrary axes.
Definition: frames.cpp:293
static Rotation EulerZYZ(double Alfa, double Beta, double Gamma)
Gives back a rotation matrix specified with EulerZYZ convention :
Definition: frames.cpp:262
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
void UnitY(const Vector &X)
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:527
static Rotation RotZ(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:615
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
void DoRotY(double angle)
The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot.
Definition: frames.inl:572
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:237
represents both translational and rotational velocities.
Definition: frames.hpp:720
friend Twist operator*(const Twist &lhs, double rhs)
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...
Twist(const Vector &_vel, const Vector &_rot)
Definition: frames.hpp:729
Twist & operator+=(const Twist &arg)
Definition: frames.inl:319
double & operator()(int i)
index-based access to components, first vel(0..2), then rot(3..5)
Definition: frames.inl:326
friend double dot(const Twist &lhs, const Wrench &rhs)
friend double dot(const Wrench &rhs, const Twist &lhs)
friend bool Equal(const Twist &a, const Twist &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
friend bool operator==(const Twist &a, const Twist &b)
The literal equality operator==(), also identical.
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:723
friend Twist operator-(const Twist &lhs, const Twist &rhs)
Twist()
The default constructor initialises to Zero via the constructor of Vector.
Definition: frames.hpp:727
friend void SetToZero(Twist &v)
friend Twist operator/(const Twist &lhs, double rhs)
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...
friend Twist operator-(const Twist &arg)
void ReverseSign()
Reverses the sign of the twist.
Definition: frames.inl:296
Twist RefPoint(const Vector &v_base_AB) const
Changes the reference point of the twist.
Definition: frames.inl:302
friend Twist operator*(double lhs, const Twist &rhs)
friend Twist operator+(const Twist &lhs, const Twist &rhs)
Twist & operator-=(const Twist &arg)
Definition: frames.inl:312
double operator[](int index) const
Definition: frames.hpp:740
Vector vel
The velocity of that point.
Definition: frames.hpp:722
static Twist Zero()
Definition: frames.inl:290
friend bool operator!=(const Twist &a, const Twist &b)
The literal inequality operator!=().
2D version of Vector
Definition: frames.hpp:960
friend bool operator!=(const Vector2 &a, const Vector2 &b)
The literal inequality operator!=().
static Vector2 Zero()
Definition: frames.inl:784
friend Vector2 operator-(const Vector2 &arg)
friend Vector2 operator-(const Vector2 &lhs, const Vector2 &rhs)
void ReverseSign()
Definition: frames.inl:806
friend Vector2 operator*(const Vector2 &lhs, double rhs)
friend bool operator==(const Vector2 &a, const Vector2 &b)
The literal equality operator==(), also identical.
double x() const
Definition: frames.inl:799
void Set3DZX(const Vector &v)
projects v in its ZX plane, and sets *this to these values
Definition: frames.inl:831
Vector2 & operator=(const Vector2 &arg)
Definition: frames.inl:737
double operator[](int index) const
Equivalent to double operator()(int index) const.
Definition: frames.hpp:977
void x(double)
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:104
void y(double)
void Set3DXY(const Vector &v)
projects v in its XY plane, and sets *this to these values
Definition: frames.inl:819
Vector2()
Does not initialise to Zero().
Definition: frames.hpp:964
friend Vector2 operator/(const Vector2 &lhs, double rhs)
Vector2 & operator+=(const Vector2 &arg)
Definition: frames.inl:770
void Set3DYZ(const Vector &v)
projects v in its YZ plane, and sets *this to these values
Definition: frames.inl:825
friend void SetToZero(Vector2 &v)
friend Vector2 operator*(const Vector2 &lhs, const Vector2 &rhs)
friend Vector2 operator+(const Vector2 &lhs, const Vector2 &rhs)
Vector2 & operator-=(const Vector2 &arg)
Definition: frames.inl:777
friend bool Equal(const Vector2 &a, const Vector2 &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
double Norm(double eps=epsilon) const
Definition: frames.cpp:87
double operator()(int index) const
Access to elements, range checked when NDEBUG is not set, from 0..1.
Definition: frames.inl:788
double y() const
Definition: frames.inl:800
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:838
friend Vector2 operator*(double lhs, const Vector2 &rhs)
double data[2]
Definition: frames.hpp:961
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:161
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
friend double dot(const Vector &lhs, const Vector &rhs)
void ReverseSign()
Reverses the sign of the Vector object itself.
Definition: frames.inl:440
friend void SetToZero(Vector &v)
To have a uniform operator to put an element to zero, for scalar values and for objects.
Vector & operator-=(const Vector &arg)
subtracts a vector from the Vector object itself
Definition: frames.inl:129
friend Vector operator+(const Vector &lhs, const Vector &rhs)
friend Vector operator/(const Vector &lhs, double rhs)
Scalar division is defined.
friend bool operator==(const Vector &a, const Vector &b)
The literal equality operator==(), also identical.
friend bool Equal(const Vector &a, const Vector &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
double z() const
Definition: frames.inl:77
double operator[](int index) const
Equivalent to double operator()(int index) const.
Definition: frames.hpp:183
double y() const
Definition: frames.inl:76
friend Vector operator-(const Vector &arg)
void Set2DZX(const Vector2 &v)
a 3D vector where the 2D vector v is put in the ZX plane
Definition: frames.inl:475
Vector & operator+=(const Vector &arg)
Adds a vector from the Vector object itself.
Definition: frames.inl:120
friend Vector operator*(const Vector &lhs, const Vector &rhs)
Vector & operator=(const Vector &arg)
Assignment operator. The normal copy by value semantics.
Definition: frames.inl:49
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:147
Vector()
Does not initialise the Vector to zero. use Vector::Zero() or SetToZero for that.
Definition: frames.hpp:165
double data[3]
Definition: frames.hpp:163
double x() const
Definition: frames.inl:75
void y(double)
static Vector Zero()
Definition: frames.inl:138
void Set2DXY(const Vector2 &v)
a 3D vector where the 2D vector v is put in the XY plane
Definition: frames.inl:458
double operator()(int index) const
Access to elements, range checked when NDEBUG is not set, from 0..2.
Definition: frames.inl:143
friend Vector operator*(const Vector &lhs, double rhs)
Scalar multiplication is defined.
friend Vector operator-(const Vector &lhs, const Vector &rhs)
void z(double)
void x(double)
double Norm(double eps=epsilon) const
Definition: frames.cpp:117
friend bool operator!=(const Vector &a, const Vector &b)
The literal inequality operator!=().
void Set2DYZ(const Vector2 &v)
a 3D vector where the 2D vector v is put in the YZ plane
Definition: frames.inl:466
friend Vector operator*(double lhs, const Vector &rhs)
Scalar multiplication is defined.
represents both translational and rotational acceleration.
Definition: frames.hpp:879
friend bool Equal(const Wrench &a, const Wrench &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
Wrench & operator+=(const Wrench &arg)
Definition: frames.inl:216
friend Wrench operator+(const Wrench &lhs, const Wrench &rhs)
Wrench & operator-=(const Wrench &arg)
Definition: frames.inl:209
Vector force
Force that is applied at the origin of the current ref frame.
Definition: frames.hpp:881
friend bool operator==(const Wrench &a, const Wrench &b)
The literal equality operator==(), also identical.
friend Wrench operator/(const Wrench &lhs, double rhs)
Scalar division.
Wrench()
Does initialise force and torque to zero via the underlying constructor of Vector.
Definition: frames.hpp:886
static Wrench Zero()
Definition: frames.inl:185
Wrench(const Vector &_force, const Vector &_torque)
Definition: frames.hpp:887
double & operator()(int i)
index-based access to components, first force(0..2), then torque(3..5)
Definition: frames.inl:223
friend Wrench operator-(const Wrench &lhs, const Wrench &rhs)
friend Wrench operator-(const Wrench &arg)
An unary - operator.
friend bool operator!=(const Wrench &a, const Wrench &b)
The literal inequality operator!=().
Vector torque
Torque that is applied at the origin of the current ref frame.
Definition: frames.hpp:882
double operator[](int index) const
Definition: frames.hpp:900
Wrench RefPoint(const Vector &v_base_AB) const
Changes the reference point of the wrench.
Definition: frames.inl:197
friend Wrench operator*(const Wrench &lhs, double rhs)
Scalar multiplication.
friend Wrench operator*(double lhs, const Wrench &rhs)
Scalar multiplication.
friend void SetToZero(Wrench &v)
Sets the Wrench to Zero, to have a uniform function that sets an object or double to zero.
void ReverseSign()
Reverses the sign of the current Wrench.
Definition: frames.inl:191
Inlined member functions and global functions that relate to the classes in frames....
Definition: articulatedbodyinertia.cpp:26
bool operator==(const Rotation &a, const Rotation &b)
Definition: frames.cpp:430
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.
IMETHOD Vector addDelta(const Vector &p_w_a, const Vector &p_w_da, double dt=1)
adds vector da to vector a.
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)