orocos_kdl
geometry.cpp
Go to the documentation of this file.
1 #include <frames.hpp>
2 #include <frames_io.hpp>
3 #include <utilities/utility.h>
4 
5 int main()
6 {
7  //Creating Vectors
8  KDL::Vector v1;//Default constructor
9  KDL::Vector v2(1.0,2.0,3.0);//Most used constructor
10  KDL::Vector v3(v2);//Copy constructor
11  KDL::Vector v4 = KDL::Vector::Zero();//Static member
12 
13  //Use operator << to print the values of your vector
14  std::cout<<"v1 ="<<v1<<std::endl;
15  std::cout<<"v2 = "<<v2<<std::endl;
16  std::cout<<"v3 = "<<v3<<std::endl;
17  std::cout<<"v4 = "<<v4<<std::endl;
18 
19  //Get/Set values of a vector
20  v1[0]=4.0;
21  v1[1]=5.0;
22  v1[2]=6.0;
23  v2(0)=7.0;
24  v2(1)=8.0;
25  v2(2)=9.0;
26  v3.x(10.0);
27  v3.y(11.0);
28  v3.z(12.0);
29 
30  std::cout<<"v1: "<<v1[0]<<", "<<v1[1]<<", "<<v1[2]<<std::endl;
31  std::cout<<"v2: "<<v2(0)<<", "<<v2(1)<<", "<<v2(2)<<std::endl;
32  std::cout<<"v3: "<<v3.x()<<", "<<v3.y()<<", "<<v3.z()<<std::endl;
33 
34  //double - vector operators
35  std::cout<<"2*v2 = "<<2*v2<<std::endl;
36  std::cout<<"v1*2 = "<<v1*2<<std::endl;
37  std::cout<<"v1/2 = "<<v1/2<<std::endl;
38 
39  //vector - vector operators
40  std::cout<<"v1+v2 = "<<v1+v2<<std::endl;
41  std::cout<<"v3-v1 = "<<v3-v1<<std::endl;
42 
43  v3-=v1;
44  v2+=v1;
45  std::cout<<"v3-=v1; v3 = "<<v3<<std::endl;
46  std::cout<<"v2+=v1; v2 = "<<v2<<std::endl;
47 
48  //cross and dot product between two vectors
49  std::cout<<"cross(v1,v2) = "<<v1*v2<<std::endl;
50  std::cout<<"dot(v1,v2) = "<<dot(v1,v2)<<std::endl;
51 
52  //Inversing the sign of a vector
53  v1=-v2;
54  std::cout<<"v1=-v2; v1="<<v1<<std::endl;
55  v1.ReverseSign();
56  std::cout<<"v1.ReverseSign(); v1 = "<<v1<<std::endl;
57 
58  //Equal operators
59  std::cout<<"v1==v2 ? "<<(v1==v2)<<std::endl;
60  std::cout<<"v1!=v2 ? "<<(v1!=v2)<<std::endl;
61  std::cout<<"Equal(v1,v2,1e-6) ? "<<Equal(v1,v2,1e-6)<<std::endl;
62 
63  //Calculating the norm and normalising your vector
64  std::cout<<"norm(v3): "<<v3.Norm()<<std::endl;
65  v3.Normalize();
66  std::cout<<"Normalize(v3)"<<v3<<std::endl;
67 
68  //Setting your vector to zero
69  SetToZero(v1);
70  std::cout<<"SetToZero(v1); v1 = "<<v1<<std::endl;
71 
72 
73  //Creating Rotations:
74  //Default constructor
75  KDL::Rotation r1;
76  //Creating a rotation matrix out of three unit vectors Vx, Vy,
77  //Vz. Be careful, these vectors should be normalised and
78  //orthogonal. Otherwise this can result in an inconsistent
79  //rotation matrix
80  KDL::Rotation r2(KDL::Vector(0,0,1),
81  KDL::Vector(0,-1,0),
82  KDL::Vector(-1,0,0));
83  //Creating a rotation matrix out of 9 values, Be careful, these
84  //values can result in an inconsistent rotation matrix if the
85  //resulting rows/columns are not orthogonal/normalized
86  KDL::Rotation r3(0,0,-1,1,0,0,0,-1,0);
87  //Creating an Identity rotation matrix
89  //Creating a Rotation matrix from a rotation around X
91  //Creating a Rotation matrix from a rotation around Y
93  //Creating a Rotation matrix from a rotation around Z
95  //Creating a Rotation matrix from a rotation around a arbitrary
96  //vector, the vector should not be normalised
98  //Creating a Rotation matrix from a rotation around a arbitrary
99  //vector, the vector should be normalised
100  KDL::Rotation r9=KDL::Rotation::Rot2(KDL::Vector(0.4472,0.5477,0.7071),
101  KDL::PI_4);
102  //Creating a Rotation matrix from Euler ZYZ rotation angles
104  //Creating a Rotation matrix from Euler ZYX rotation angles
106  //Creating a Rotation matrix from Roll-Pitch-Yaw rotation angles
107  KDL::Rotation r12=KDL::Rotation::RPY(1.,2.,3.);
108 
109  //Printing the rotations:
110  std::cout<<"r1: "<<r1<<std::endl;
111  std::cout<<"r2: "<<r2<<std::endl;
112  std::cout<<"r3: "<<r3<<std::endl;
113  std::cout<<"r4: "<<r4<<std::endl;
114  std::cout<<"r5: "<<r5<<std::endl;
115  std::cout<<"r6: "<<r6<<std::endl;
116  std::cout<<"r7: "<<r7<<std::endl;
117  std::cout<<"r8: "<<r8<<std::endl;
118  std::cout<<"r9: "<<r9<<std::endl;
119  std::cout<<"r10: "<<r10<<std::endl;
120  std::cout<<"r11: "<<r11<<std::endl;
121  std::cout<<"r12: "<<r12<<std::endl;
122 
123  //Getting information out of the rotation matrix:
124  //The individual elements
125  std::cout<<"r8(1,2): "<<r8(1,2)<<std::endl;
126  //The equivalent rotation vector;
127  std::cout<<"equiv rot vector of r11: "<<r11.GetRot()<<std::endl;
128  //The equivalent rotation vector and angle:
129  double angle=r10.GetRotAngle(v1);
130  std::cout<<"equiv rot vector of r10:"<<v1<<"and angle: "<<angle<<std::endl;
131  //The Euler ZYZ angles
132  double alfa,beta,gamma;
133  r9.GetEulerZYZ(alfa,beta,gamma);
134  std::cout<<"EulerZYZ: "<<alfa<<", "<<beta<<", "<<gamma<<std::endl;
135  //The Euler ZYZ angles
136  r9.GetEulerZYX(alfa,beta,gamma);
137  std::cout<<"EulerZYX: "<<alfa<<", "<<beta<<", "<<gamma<<std::endl;
138  //The Roll-Pitch-Yaw angles
139  r9.GetRPY(alfa,beta,gamma);
140  std::cout<<"Roll-Pitch-Yaw: "<<alfa<<", "<<beta<<", "<<gamma<<std::endl;
141  //The underlying unitvector X
142  r8.UnitX(v1);//or
143  std::cout<<"UnitX of r8:"<<r8.UnitX()<<std::endl;
144  //The underlying unitvector Y
145  r8.UnitY(v1);//or
146  std::cout<<"Unity of r8:"<<r8.UnitY()<<std::endl;
147  //The underlying unitvector Z
148  r8.UnitZ(v1);//or
149  std::cout<<"UnitZ of r8:"<<r8.UnitZ()<<std::endl;
150 }
KDL::Rotation::Rot2
static Rotation Rot2(const Vector &rotvec, double angle)
Along an arbitrary axes. rotvec should be normalized.
Definition: frames.cpp:328
frames.hpp
KDL::Vector::y
double y() const
Definition: frames.inl:76
main
int main()
Definition: geometry.cpp:5
KDL::Rotation::GetRot
Vector GetRot() const
Definition: frames.cpp:361
KDL::Vector::Zero
static Vector Zero()
Definition: frames.inl:138
KDL::Rotation::GetEulerZYZ
void GetEulerZYZ(double &alpha, double &beta, double &gamma) const
Definition: frames.cpp:300
frames_io.hpp
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::Rotation::GetRPY
void GetRPY(double &roll, double &pitch, double &yaw) const
Definition: frames.cpp:274
utility.h
KDL::Vector::Normalize
double Normalize(double eps=epsilon)
Definition: frames.cpp:172
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:162
std::cout
KDL::Rotation::GetEulerZYX
void GetEulerZYX(double &Alfa, double &Beta, double &Gamma) const
Definition: frames.hpp:495
KDL::Vector::z
double z() const
Definition: frames.inl:77
dot
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
Definition: frameacc.inl:137
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::PI
const double PI
the value of pi
Definition: utility.cxx:16
KDL::Rotation::GetRotAngle
double GetRotAngle(Vector &axis, double eps=epsilon) const
Definition: frames.cpp:383
KDL::Rotation::EulerZYZ
static Rotation EulerZYZ(double Alfa, double Beta, double Gamma)
Definition: frames.cpp:287
std::endl
T endl(T... args)
KDL::Rotation::EulerZYX
static Rotation EulerZYX(double Alfa, double Beta, double Gamma)
Definition: frames.hpp:471
KDL::Rotation::Rot
static Rotation Rot(const Vector &rotvec, double angle)
Definition: frames.cpp:318
KDL::Equal
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
KDL::Rotation::UnitY
Vector UnitY() const
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:524
KDL::Vector::ReverseSign
void ReverseSign()
Reverses the sign of the Vector object itself.
Definition: frames.inl:440
SetToZero
IMETHOD void SetToZero(Vector &v)
Definition: frames.inl:1068
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::Rotation::RPY
static Rotation RPY(double roll, double pitch, double yaw)
Definition: frames.cpp:262
KDL::Vector::x
double x() const
Definition: frames.inl:75
KDL::Rotation::UnitZ
Vector UnitZ() const
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:536
KDL::Rotation::UnitX
Vector UnitX() const
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:512
KDL::PI_4
const double PI_4
the value of pi/4
Definition: utility.cxx:18
KDL::Rotation
represents rotations in 3 dimensional space.
Definition: frames.hpp:303