utilities.hpp

00001 #ifndef _utilities_hpp
00002 #define _utilities_hpp
00003 
00004 
00005 #include "Core.h"
00006 #ifndef M_PI
00007 #define M_PI 3.14159265358979
00008 #endif
00009 
00010 using namespace std;  //calling the standard directory
00011 using namespace Eigen;
00012 
00013 namespace utilities{ 
00014 
00015     inline Matrix3f rotx(float alpha){
00016         Matrix3f Rx;
00017         Rx <<   1,0,0,
00018             0,cos(alpha),-sin(alpha),
00019             0,sin(alpha), cos(alpha);
00020         return Rx;
00021     }
00022 
00023     inline Matrix3f roty(float beta){
00024         Matrix3f Ry;
00025         Ry <<   cos(beta),0,sin(beta),
00026             0,1,0,
00027             -sin(beta),0, cos(beta);
00028         return Ry;
00029     }
00030 
00031     inline Matrix3f rotz(float gamma){
00032         Matrix3f Rz;
00033         Rz <<   cos(gamma),-sin(gamma),0,
00034             sin(gamma),cos(gamma),0,
00035             0,0, 1;
00036         return Rz;
00037     }
00038 
00039 
00040     inline Matrix4f rotx_T(float alpha){
00041         Matrix4f Tx = Matrix4f::Identity();
00042         Tx.block(0,0,3,3) = rotx(alpha);
00043         return Tx;
00044     }
00045 
00046     inline Matrix4f roty_T(float beta){
00047         Matrix4f Ty = Matrix4f::Identity();
00048         Ty.block(0,0,3,3) = roty(beta);
00049         return Ty;
00050     }
00051 
00052     inline Matrix4f rotz_T(float gamma){
00053         Matrix4f Tz = Matrix4f::Identity();
00054         Tz.block(0,0,3,3) = rotz(gamma);
00055         return Tz;
00056     }
00057 
00058     inline Matrix3f skew(Vector3f v)
00059     {
00060         Matrix3f S;
00061         S << 0, -v[2],   v[1],      //Skew-symmetric matrix
00062             v[2],       0,  -v[0],
00063             -v[1],   v[0],     0;
00064         return S;
00065     }
00066 
00067 
00068     inline Matrix3f L_matrix(Matrix3f R_d, Matrix3f R_e)
00069     {
00070         Matrix3f L = -0.5 * (skew(R_d.col(0))*skew(R_e.col(0)) + skew(R_d.col(1))*skew(R_e.col(1)) + skew(R_d.col(2))*skew(R_e.col(2)));
00071         return L;
00072     }
00073 
00074 
00075 
00076     inline Vector3f rotationMatrixError(Matrix4f Td, Matrix4f Te)
00077     {
00078         
00079         Matrix3f R_e = Te.block(0,0,3,3);       //Matrix.slice<RowStart, ColStart, NumRows, NumCols>(); 
00080         Matrix3f R_d = Td.block(0,0,3,3);
00081         
00082         Vector3f eo = 0.5 * (skew(R_e.col(0))*R_d.col(0) + skew(R_e.col(1))*R_d.col(1) + skew(R_e.col(2))*R_d.col(2)) ;
00083         return eo;
00084     }
00085 
00086 
00087     inline Vector3f r2quat(Matrix3f R_iniz, float &eta)
00088     {
00089         Vector3f epsilon;
00090         int iu, iv, iw;
00091 
00092         if ( (R_iniz(0,0) >= R_iniz(1,1)) && (R_iniz(0,0) >= R_iniz(2,2)) )
00093         {
00094             iu = 0; iv = 1; iw = 2;
00095         }
00096         else if ( (R_iniz(1,1) >= R_iniz(0,0)) && (R_iniz(1,1) >= R_iniz(2,2)) )
00097         {
00098             iu = 1; iv = 2; iw = 0;
00099         }
00100         else
00101         {
00102             iu = 2; iv = 0; iw = 1;
00103         }
00104 
00105         float r = sqrt(1 + R_iniz(iu,iu) - R_iniz(iv,iv) - R_iniz(iw,iw));
00106         Vector3f q;
00107         q <<  0,0,0;
00108         if (r>0)
00109         {
00110         float rr = 2*r;
00111         eta = (R_iniz(iw,iv)-R_iniz(iv,iw)/rr);
00112         epsilon[iu] = r/2;
00113         epsilon[iv] = (R_iniz(iu,iv)+R_iniz(iv,iu))/rr;
00114         epsilon[iw] = (R_iniz(iw,iu)+R_iniz(iu,iw))/rr;
00115         }
00116         else
00117         {
00118         eta = 1;
00119         epsilon << 0,0,0;
00120         }
00121         return epsilon;
00122     }
00123 
00124 
00125     inline Vector4f rot2quat(Matrix3f R){
00126 
00127         float m00, m01, m02, m10, m11, m12, m20, m21, m22;
00128 
00129         m00 = R(0,0);
00130         m01 = R(0,1);
00131         m02 = R(0,2);
00132         m10 = R(1,0);
00133         m11 = R(1,1);
00134         m12 = R(1,2);
00135         m20 = R(2,0);
00136         m21 = R(2,1);
00137         m22 = R(2,2);
00138 
00139         float tr = m00 + m11 + m22;
00140         float qw, qx, qy, qz, S;
00141         Vector4f quat;
00142 
00143         if (tr > 0) { 
00144           S = sqrt(tr+1.0) * 2; // S=4*qw 
00145           qw = 0.25 * S;
00146           qx = (m21 - m12) / S;
00147           qy = (m02 - m20) / S; 
00148           qz = (m10 - m01) / S; 
00149         } else if ((m00 > m11)&(m00 > m22)) { 
00150           S = sqrt(1.0 + m00 - m11 - m22) * 2; // S=4*qx 
00151           qw = (m21 - m12) / S;
00152           qx = 0.25 * S;
00153           qy = (m01 + m10) / S; 
00154           qz = (m02 + m20) / S; 
00155         } else if (m11 > m22) { 
00156           S = sqrt(1.0 + m11 - m00 - m22) * 2; // S=4*qy
00157           qw = (m02 - m20) / S;
00158           qx = (m01 + m10) / S; 
00159           qy = 0.25 * S;
00160           qz = (m12 + m21) / S; 
00161         } else { 
00162           S = sqrt(1.0 + m22 - m00 - m11) * 2; // S=4*qz
00163           qw = (m10 - m01) / S;
00164           qx = (m02 + m20) / S;
00165           qy = (m12 + m21) / S;
00166           qz = 0.25 * S;
00167         }
00168 
00169         quat << qw, qx, qy, qz;
00170         return quat;
00171 
00172     }
00173     
00174     
00175          //Matrix ortonormalization
00176     inline Matrix3f matrixOrthonormalization(Matrix3f R){
00177 
00178         SelfAdjointEigenSolver<Matrix3f> es(R.transpose()*R);
00179         Vector3f D = es.eigenvalues();
00180         Matrix3f V = es.eigenvectors();
00181         R = R*((1/sqrt(D(0)))*V.col(0)*V.col(0).transpose() + (1/sqrt(D(1)))*V.col(1)*V.col(1).transpose() + (1/sqrt(D(2)))*V.col(2)*V.col(2).transpose());
00182 
00183         return R;
00184     }
00185 
00186 
00187 
00188 
00189     //******************************************************************************
00190     inline Vector3f quaternionError(Matrix4f Tbe, Matrix4f Tbe_d)
00191     {
00192         float eta, eta_d;
00193         Matrix3f R = Tbe.block(0,0,3,3);        //Matrix.slice<RowStart, ColStart, NumRows, NumCols>(); 
00194         Vector3f epsilon = r2quat(R, eta);
00195         Matrix3f R_d = Tbe_d.block(0,0,3,3);
00196         Vector3f epsilon_d = r2quat(R_d, eta_d);
00197         Matrix3f S = skew(epsilon_d);
00198         Vector3f eo = eta*epsilon_d-eta_d*epsilon-S*epsilon;
00199         return eo;
00200     }
00201 
00202 
00203     inline Vector3f versorError(Vector3f P_d, Matrix3f Tbe_e, float &theta)
00204     {
00205         Vector3f P_e = Tbe_e.block(0,3,3,1);
00206         Vector3f u_e = Tbe_e.block(0,0,3,1);
00207 
00208         Vector3f u_d = P_d - P_e;
00209         u_d = u_d/ u_d.norm();
00210         
00211         Vector3f r = skew(u_e)*u_d;
00212         float nr = r.norm();
00213             
00214       if (nr >0){
00215             r = r/r.norm();
00216             
00217 
00218             //be carefult to acos( > 1 )
00219             float u_e_d = u_e.transpose()*u_d;      
00220             if( fabs(u_e_d) <= 1.0 ) {
00221                 theta = acos(u_e.transpose()*u_d);
00222             }
00223             else {
00224                 theta = 0.0;
00225             }
00226             
00227             Vector3f error = r*sin(theta);
00228             return error;
00229         }else{
00230             theta = 0.0;
00231             return Vector3f::Zero();
00232         }
00233     }
00234 
00235 
00236     /*Matrix3f utilities::rotation ( float theta, Vector3f r ) {
00237          
00238         Matrix3f R = Zeros;
00239      
00240         R[0][0] = r[0]*r[0]*(1-cos(theta)) + cos(theta);
00241         R[1][1] = r[1]*r[1]*(1-cos(theta)) + cos(theta);
00242         R[2][2] = r[2]*r[2]*(1-cos(theta)) + cos(theta);
00243      
00244         R[0][1] = r[0]*r[1]*(1-cos(theta)) - r[2]*sin(theta);
00245         R[1][0] = r[0]*r[1]*(1-cos(theta)) + r[2]*sin(theta);
00246      
00247         R[0][2] = r[0]*r[2]*(1-cos(theta)) + r[1]*sin(theta);
00248         R[2][0] = r[0]*r[2]*(1-cos(theta)) - r[1]*sin(theta);
00249          
00250         R[1][2] = r[1]*r[2]*(1-cos(theta)) - r[0]*sin(theta);
00251         R[2][1] = r[1]*r[2]*(1-cos(theta)) + r[0]*sin(theta);
00252          
00253      
00254         return R;
00255     }*/
00256 
00257 
00258     inline Matrix3f XYZ2R(Vector3f angles) {
00259         
00260         Matrix3f R = Matrix3f::Zero(); 
00261         Matrix3f R1 = Matrix3f::Zero(); 
00262         Matrix3f R2 = Matrix3f::Zero(); 
00263         Matrix3f R3 = Matrix3f::Zero();
00264 
00265         float cos_phi = cos(angles[0]);
00266         float sin_phi = sin(angles[0]);
00267         float cos_theta = cos(angles[1]);
00268         float sin_theta = sin(angles[1]);
00269         float cos_psi = cos(angles[2]);
00270         float sin_psi = sin(angles[2]);
00271 
00272         R1  << 1, 0      , 0, 
00273                     0, cos_phi, -sin_phi, 
00274                     0, sin_phi, cos_phi;
00275 
00276         R2  << cos_theta , 0, sin_theta, 
00277                     0        , 1, 0       , 
00278                     -sin_theta, 0, cos_theta;
00279 
00280         R3  << cos_psi, -sin_psi, 0, 
00281                     sin_psi, cos_psi , 0,
00282                     0      , 0       , 1;
00283 
00284         R = R1*R2*R3;
00285 
00286         return R;
00287     }
00288 
00289     // This method computes the XYZ Euler angles from the Rotational matrix R.
00290     inline Vector3f R2XYZ(Matrix3f R) {
00291         float phi=0.0, theta=0.0, psi=0.0;
00292         Vector3f XYZ = Vector3f::Zero();
00293         
00294         theta = asin(R(0,2));
00295         
00296         if(fabsf(cos(theta))>pow(10.0,-10.0))
00297         {
00298             phi=atan2(-R(1,2)/cos(theta), R(2,2)/cos(theta));
00299             psi=atan2(-R(0,1)/cos(theta), R(0,0)/cos(theta));
00300         }
00301         else
00302         {
00303             if(fabsf(theta-M_PI/2.0)<pow(10.0,-5.0))
00304             {
00305                 psi = 0.0;
00306                 phi = atan2(R(1,0), R(2,0));
00307                 theta = M_PI/2.0;
00308             }
00309             else
00310             {
00311                 psi = 0.0;
00312                 phi = atan2(-R(1,0), R(2,0));
00313                 theta = -M_PI/2.0;
00314             }
00315         }
00316         
00317         XYZ << phi,theta,psi;
00318         return XYZ;
00319     }
00320 
00321     inline Matrix3f angleAxis2Rot(Vector3f ri, float theta){
00322     Matrix3f R;
00323     R << ri[0]*ri[0] * (1 - cos(theta)) + cos(theta)           , ri[0] * ri[1] * (1 - cos(theta)) - ri[2] * sin(theta) , ri[0] * ri[2] * (1 - cos(theta)) + ri[1] * sin(theta),
00324              ri[0] * ri[1] * (1 - cos(theta)) + ri[2] * sin(theta) , ri[1]*ri[1] * (1 - cos(theta)) + cos(theta)           , ri[1] * ri[2] * (1 - cos(theta)) - ri[0] * sin(theta),
00325              ri[0] * ri[2] * (1 - cos(theta)) - ri[1] * sin(theta) , ri[1] * ri[2] * (1 - cos(theta)) + ri[0] * sin(theta) , ri[2]*ri[2] * (1 - cos(theta)) + cos(theta);
00326 
00327     return R;
00328 
00329     }
00330 
00331 
00332 
00333     inline Vector3f butt_filter(Vector3f x, Vector3f x1, Vector3f x2, float omega_n, float zita, float ctrl_T){
00334         //applico un filtro di Butterworth del secondo ordine (sfrutto Eulero all'indietro)
00335         return x1*(2.0 + 2.0*omega_n*zita*ctrl_T)/(omega_n*omega_n*ctrl_T*ctrl_T + 2.0*omega_n*zita*ctrl_T + 1.0) - x2/(omega_n*omega_n*ctrl_T*ctrl_T + 2.0*omega_n*zita*ctrl_T + 1.0) + x*(omega_n*omega_n*ctrl_T*ctrl_T)/(omega_n*omega_n*ctrl_T*ctrl_T + 2.0*omega_n*zita*ctrl_T + 1.0);
00336     }
00337 
00338 
00339 
00340     /// converts a rate in Hz to an integer period in ms.
00341     inline uint16_t rateToPeriod(const float & rate) {
00342         if (rate > 0)
00343             return static_cast<uint16_t> (1000.0 / rate);
00344         else
00345             return 0;
00346     }
00347 
00348 
00349 
00350 
00351         //Quaternion to rotration Matrix
00352     inline Matrix3f QuatToMat(Vector4f Quat){
00353         Matrix3f Rot;
00354         float s = Quat[0];
00355         float x = Quat[1];
00356         float y = Quat[2];
00357         float z = Quat[3];
00358         Rot << 1-2*(y*y+z*z),2*(x*y-s*z),2*(x*z+s*y),
00359         2*(x*y+s*z),1-2*(x*x+z*z),2*(y*z-s*x),
00360         2*(x*z-s*y),2*(y*z+s*x),1-2*(x*x+y*y);
00361         return Rot;
00362     }
00363     
00364     
00365     inline float rad2deg(float rad){
00366         float deg;
00367         deg = 180.0*rad/M_PI;
00368         return deg;
00369     }
00370     
00371     inline float deg2rad(float deg){
00372         float rad;
00373         rad = M_PI*deg/180.0;
00374         return rad;
00375     }
00376 }
00377 
00378 #endif
00379