geometry.cpp
Go to the documentation of this file.
00001 #include <frames.hpp>
00002 #include <frames_io.hpp>
00003 
00004 int main()
00005 {
00006 
00007     //Creating Vectors
00008     KDL::Vector v1;//Default constructor
00009     KDL::Vector v2(1.0,2.0,3.0);//Most used constructor
00010     KDL::Vector v3(v2);//Copy constructor
00011     KDL::Vector v4 = KDL::Vector::Zero();//Static member
00012     
00013     //Use operator << to print the values of your vector
00014     std::cout<<"v1 ="<<v1<<std::endl;
00015     std::cout<<"v2 = "<<v2<<std::endl;
00016     std::cout<<"v3 = "<<v3<<std::endl;
00017     std::cout<<"v4 = "<<v4<<std::endl;
00018     
00019     //Get/Set values of a vector
00020     v1[0]=4.0;
00021     v1[1]=5.0;
00022     v1[2]=6.0;
00023     v2(0)=7.0;
00024     v2(1)=8.0;
00025     v2(2)=9.0;
00026     v3.x(10.0);
00027     v3.y(11.0);
00028     v3.z(12.0);
00029     
00030     std::cout<<"v1: "<<v1[0]<<", "<<v1[1]<<", "<<v1[2]<<std::endl;
00031     std::cout<<"v2: "<<v2(0)<<", "<<v2(1)<<", "<<v2(2)<<std::endl;
00032     std::cout<<"v3: "<<v3.x()<<", "<<v3.y()<<", "<<v3.z()<<std::endl;
00033 
00034     //double - vector operators
00035     std::cout<<"2*v2 = "<<2*v2<<std::endl;
00036     std::cout<<"v1*2 = "<<v1*2<<std::endl;
00037     std::cout<<"v1/2 = "<<v1/2<<std::endl;
00038     
00039     //vector - vector operators
00040     std::cout<<"v1+v2 = "<<v1+v2<<std::endl;
00041     std::cout<<"v3-v1 = "<<v3-v1<<std::endl;
00042 
00043     v3-=v1;
00044     v2+=v1;    
00045     std::cout<<"v3-=v1; v3 = "<<v3<<std::endl;
00046     std::cout<<"v2+=v1; v2 = "<<v2<<std::endl;
00047 
00048     //cross and dot product between two vectors
00049     std::cout<<"cross(v1,v2) =  "<<v1*v2<<std::endl;
00050     std::cout<<"dot(v1,v2) = "<<dot(v1,v2)<<std::endl;
00051 
00052     //Inversing the sign of a vector
00053     v1=-v2;
00054     std::cout<<"v1=-v2; v1="<<v1<<std::endl;
00055     v1.ReverseSign();
00056     std::cout<<"v1.ReverseSign(); v1 = "<<v1<<std::endl;
00057     
00058     //Equal operators
00059     std::cout<<"v1==v2 ? "<<(v1==v2)<<std::endl;
00060     std::cout<<"v1!=v2 ? "<<(v1!=v2)<<std::endl;
00061     std::cout<<"Equal(v1,v2,1e-6) ? "<<Equal(v1,v2,1e-6)<<std::endl;
00062     
00063     //Calculating the norm and normalising your vector
00064     std::cout<<"norm(v3): "<<v3.Norm()<<std::endl;
00065     v3.Normalize();
00066     std::cout<<"Normalize(v3)"<<v3<<std::endl;
00067         
00068     //Setting your vector to zero
00069     SetToZero(v1);
00070     std::cout<<"SetToZero(v1); v1 = "<<v1<<std::endl;
00071 
00072 
00073     //Creating Rotations:
00074     //Default constructor
00075     KDL::Rotation r1;
00076     //Creating a rotation matrix out of three unit vectors Vx, Vy,
00077     //Vz. Be careful, these vectors should be normalised and
00078     //orthogonal. Otherwise this can result in an inconsistent
00079     //rotation matrix
00080     KDL::Rotation r2(KDL::Vector(0,0,1),
00081                      KDL::Vector(0,-1,0),
00082                      KDL::Vector(-1,0,0));
00083     //Creating a rotation matrix out of 9 values, Be careful, these
00084     //values can result in an inconsistent rotation matrix if the
00085     //resulting rows/columns are not orthogonal/normalized
00086     KDL::Rotation r3(0,0,-1,1,0,0,0,-1,0);
00087     //Creating an Identity rotation matrix
00088     KDL::Rotation r4=KDL::Rotation::Identity();
00089     //Creating a Rotation matrix from a rotation around X
00090     KDL::Rotation r5=KDL::Rotation::RotX(M_PI/3);
00091     //Creating a Rotation matrix from a rotation around Y
00092     KDL::Rotation r6=KDL::Rotation::RotY(M_PI/3);
00093     //Creating a Rotation matrix from a rotation around Z
00094     KDL::Rotation r7=KDL::Rotation::RotZ(M_PI/3);
00095     //Creating a Rotation matrix from a rotation around a arbitrary
00096     //vector, the vector should not be normalised
00097     KDL::Rotation r8=KDL::Rotation::Rot(KDL::Vector(1.,2.,3.),M_PI/4);
00098     //Creating a Rotation matrix from a rotation around a arbitrary
00099     //vector, the vector should be normalised
00100     KDL::Rotation r9=KDL::Rotation::Rot2(KDL::Vector(0.4472,0.5477,0.7071),
00101                                          M_PI/4);
00102     //Creating a Rotation matrix from Euler ZYZ rotation angles
00103     KDL::Rotation r10=KDL::Rotation::EulerZYZ(1.,2.,3.);
00104     //Creating a Rotation matrix from Euler ZYX rotation angles
00105     KDL::Rotation r11=KDL::Rotation::EulerZYX(1.,2.,3.);
00106     //Creating a Rotation matrix from Roll-Pitch-Yaw rotation angles
00107     KDL::Rotation r12=KDL::Rotation::RPY(1.,2.,3.);
00108     
00109     //Printing the rotations:
00110     std::cout<<"r1: "<<r1<<std::endl;
00111     std::cout<<"r2: "<<r2<<std::endl;
00112     std::cout<<"r3: "<<r3<<std::endl;
00113     std::cout<<"r4: "<<r4<<std::endl;
00114     std::cout<<"r5: "<<r5<<std::endl;
00115     std::cout<<"r6: "<<r6<<std::endl;
00116     std::cout<<"r7: "<<r7<<std::endl;
00117     std::cout<<"r8: "<<r8<<std::endl;
00118     std::cout<<"r9: "<<r9<<std::endl;
00119     std::cout<<"r10: "<<r10<<std::endl;
00120     std::cout<<"r11: "<<r11<<std::endl;
00121     std::cout<<"r12: "<<r12<<std::endl;
00122         
00123     //Getting information out of the rotation matrix:
00124     //The individual elements
00125     std::cout<<"r8(1,2): "<<r8(1,2)<<std::endl;
00126     //The equivalent rotation vector;
00127     std::cout<<"equiv rot vector of r11: "<<r11.GetRot()<<std::endl;
00128     //The equivalent rotation vector and angle:
00129     double angle=r10.GetRotAngle(v1);
00130     std::cout<<"equiv rot vector of r10:"<<v1<<"and angle: "<<angle<<std::endl;
00131     //The Euler ZYZ angles
00132     double alfa,beta,gamma;
00133     r9.GetEulerZYZ(alfa,beta,gamma);
00134     std::cout<<"EulerZYZ: "<<alfa<<", "<<beta<<", "<<gamma<<std::endl;
00135     //The Euler ZYZ angles
00136     r9.GetEulerZYX(alfa,beta,gamma);
00137     std::cout<<"EulerZYX: "<<alfa<<", "<<beta<<", "<<gamma<<std::endl;
00138     //The Roll-Pitch-Yaw angles
00139     r9.GetRPY(alfa,beta,gamma);
00140     std::cout<<"Roll-Pitch-Yaw: "<<alfa<<", "<<beta<<", "<<gamma<<std::endl;
00141     //The underlying unitvector X
00142     r8.UnitX(v1);//or
00143     std::cout<<"UnitX of r8:"<<r8.UnitX()<<std::endl;
00144     //The underlying unitvector Y
00145     r8.UnitY(v1);//or
00146     std::cout<<"Unity of r8:"<<r8.UnitY()<<std::endl;
00147     //The underlying unitvector Z
00148     r8.UnitZ(v1);//or
00149     std::cout<<"UnitZ of r8:"<<r8.UnitZ()<<std::endl;
00150     
00151     
00152 
00153         
00154 }    


orocos_kdl
Author(s):
autogenerated on Fri Jun 14 2019 19:33:22