prima prova bracctio
Dependencies: Eigen MX64_senzaCorrente
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
Generated on Mon Jul 18 2022 17:05:26 by 1.7.2