00001 #include <frames.hpp>
00002 #include <frames_io.hpp>
00003
00004 int main()
00005 {
00006
00007
00008 KDL::Vector v1;
00009 KDL::Vector v2(1.0,2.0,3.0);
00010 KDL::Vector v3(v2);
00011 KDL::Vector v4 = KDL::Vector::Zero();
00012
00013
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
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
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
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
00049 std::cout<<"cross(v1,v2) = "<<v1*v2<<std::endl;
00050 std::cout<<"dot(v1,v2) = "<<dot(v1,v2)<<std::endl;
00051
00052
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
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
00064 std::cout<<"norm(v3): "<<v3.Norm()<<std::endl;
00065 v3.Normalize();
00066 std::cout<<"Normalize(v3)"<<v3<<std::endl;
00067
00068
00069 SetToZero(v1);
00070 std::cout<<"SetToZero(v1); v1 = "<<v1<<std::endl;
00071
00072
00073
00074
00075 KDL::Rotation r1;
00076
00077
00078
00079
00080 KDL::Rotation r2(KDL::Vector(0,0,1),
00081 KDL::Vector(0,-1,0),
00082 KDL::Vector(-1,0,0));
00083
00084
00085
00086 KDL::Rotation r3(0,0,-1,1,0,0,0,-1,0);
00087
00088 KDL::Rotation r4=KDL::Rotation::Identity();
00089
00090 KDL::Rotation r5=KDL::Rotation::RotX(M_PI/3);
00091
00092 KDL::Rotation r6=KDL::Rotation::RotY(M_PI/3);
00093
00094 KDL::Rotation r7=KDL::Rotation::RotZ(M_PI/3);
00095
00096
00097 KDL::Rotation r8=KDL::Rotation::Rot(KDL::Vector(1.,2.,3.),M_PI/4);
00098
00099
00100 KDL::Rotation r9=KDL::Rotation::Rot2(KDL::Vector(0.4472,0.5477,0.7071),
00101 M_PI/4);
00102
00103 KDL::Rotation r10=KDL::Rotation::EulerZYZ(1.,2.,3.);
00104
00105 KDL::Rotation r11=KDL::Rotation::EulerZYX(1.,2.,3.);
00106
00107 KDL::Rotation r12=KDL::Rotation::RPY(1.,2.,3.);
00108
00109
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
00124
00125 std::cout<<"r8(1,2): "<<r8(1,2)<<std::endl;
00126
00127 std::cout<<"equiv rot vector of r11: "<<r11.GetRot()<<std::endl;
00128
00129 double angle=r10.GetRotAngle(v1);
00130 std::cout<<"equiv rot vector of r10:"<<v1<<"and angle: "<<angle<<std::endl;
00131
00132 double alfa,beta,gamma;
00133 r9.GetEulerZYZ(alfa,beta,gamma);
00134 std::cout<<"EulerZYZ: "<<alfa<<", "<<beta<<", "<<gamma<<std::endl;
00135
00136 r9.GetEulerZYX(alfa,beta,gamma);
00137 std::cout<<"EulerZYX: "<<alfa<<", "<<beta<<", "<<gamma<<std::endl;
00138
00139 r9.GetRPY(alfa,beta,gamma);
00140 std::cout<<"Roll-Pitch-Yaw: "<<alfa<<", "<<beta<<", "<<gamma<<std::endl;
00141
00142 r8.UnitX(v1);
00143 std::cout<<"UnitX of r8:"<<r8.UnitX()<<std::endl;
00144
00145 r8.UnitY(v1);
00146 std::cout<<"Unity of r8:"<<r8.UnitY()<<std::endl;
00147
00148 r8.UnitZ(v1);
00149 std::cout<<"UnitZ of r8:"<<r8.UnitZ()<<std::endl;
00150
00151
00152
00153
00154 }