prima prova bracctio
Dependencies: Eigen MX64_senzaCorrente
ARM/utilities.hpp
- Committer:
- anfontanelli
- Date:
- 2019-11-18
- Revision:
- 0:82a5f03b7eb4
File content as of revision 0:82a5f03b7eb4:
#ifndef _utilities_hpp #define _utilities_hpp #include "Core.h" #ifndef M_PI #define M_PI 3.14159265358979 #endif using namespace std; //calling the standard directory using namespace Eigen; namespace utilities{ inline Matrix3f rotx(float alpha){ Matrix3f Rx; Rx << 1,0,0, 0,cos(alpha),-sin(alpha), 0,sin(alpha), cos(alpha); return Rx; } inline Matrix3f roty(float beta){ Matrix3f Ry; Ry << cos(beta),0,sin(beta), 0,1,0, -sin(beta),0, cos(beta); return Ry; } inline Matrix3f rotz(float gamma){ Matrix3f Rz; Rz << cos(gamma),-sin(gamma),0, sin(gamma),cos(gamma),0, 0,0, 1; return Rz; } inline Matrix4f rotx_T(float alpha){ Matrix4f Tx = Matrix4f::Identity(); Tx.block(0,0,3,3) = rotx(alpha); return Tx; } inline Matrix4f roty_T(float beta){ Matrix4f Ty = Matrix4f::Identity(); Ty.block(0,0,3,3) = roty(beta); return Ty; } inline Matrix4f rotz_T(float gamma){ Matrix4f Tz = Matrix4f::Identity(); Tz.block(0,0,3,3) = rotz(gamma); return Tz; } inline Matrix3f skew(Vector3f v) { Matrix3f S; S << 0, -v[2], v[1], //Skew-symmetric matrix v[2], 0, -v[0], -v[1], v[0], 0; return S; } inline Matrix3f L_matrix(Matrix3f R_d, Matrix3f R_e) { 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))); return L; } inline Vector3f rotationMatrixError(Matrix4f Td, Matrix4f Te) { Matrix3f R_e = Te.block(0,0,3,3); //Matrix.slice<RowStart, ColStart, NumRows, NumCols>(); Matrix3f R_d = Td.block(0,0,3,3); 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)) ; return eo; } inline Vector3f r2quat(Matrix3f R_iniz, float &eta) { Vector3f epsilon; int iu, iv, iw; if ( (R_iniz(0,0) >= R_iniz(1,1)) && (R_iniz(0,0) >= R_iniz(2,2)) ) { iu = 0; iv = 1; iw = 2; } else if ( (R_iniz(1,1) >= R_iniz(0,0)) && (R_iniz(1,1) >= R_iniz(2,2)) ) { iu = 1; iv = 2; iw = 0; } else { iu = 2; iv = 0; iw = 1; } float r = sqrt(1 + R_iniz(iu,iu) - R_iniz(iv,iv) - R_iniz(iw,iw)); Vector3f q; q << 0,0,0; if (r>0) { float rr = 2*r; eta = (R_iniz(iw,iv)-R_iniz(iv,iw)/rr); epsilon[iu] = r/2; epsilon[iv] = (R_iniz(iu,iv)+R_iniz(iv,iu))/rr; epsilon[iw] = (R_iniz(iw,iu)+R_iniz(iu,iw))/rr; } else { eta = 1; epsilon << 0,0,0; } return epsilon; } inline Vector4f rot2quat(Matrix3f R){ float m00, m01, m02, m10, m11, m12, m20, m21, m22; m00 = R(0,0); m01 = R(0,1); m02 = R(0,2); m10 = R(1,0); m11 = R(1,1); m12 = R(1,2); m20 = R(2,0); m21 = R(2,1); m22 = R(2,2); float tr = m00 + m11 + m22; float qw, qx, qy, qz, S; Vector4f quat; if (tr > 0) { S = sqrt(tr+1.0) * 2; // S=4*qw qw = 0.25 * S; qx = (m21 - m12) / S; qy = (m02 - m20) / S; qz = (m10 - m01) / S; } else if ((m00 > m11)&(m00 > m22)) { S = sqrt(1.0 + m00 - m11 - m22) * 2; // S=4*qx qw = (m21 - m12) / S; qx = 0.25 * S; qy = (m01 + m10) / S; qz = (m02 + m20) / S; } else if (m11 > m22) { S = sqrt(1.0 + m11 - m00 - m22) * 2; // S=4*qy qw = (m02 - m20) / S; qx = (m01 + m10) / S; qy = 0.25 * S; qz = (m12 + m21) / S; } else { S = sqrt(1.0 + m22 - m00 - m11) * 2; // S=4*qz qw = (m10 - m01) / S; qx = (m02 + m20) / S; qy = (m12 + m21) / S; qz = 0.25 * S; } quat << qw, qx, qy, qz; return quat; } //Matrix ortonormalization inline Matrix3f matrixOrthonormalization(Matrix3f R){ SelfAdjointEigenSolver<Matrix3f> es(R.transpose()*R); Vector3f D = es.eigenvalues(); Matrix3f V = es.eigenvectors(); 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()); return R; } //****************************************************************************** inline Vector3f quaternionError(Matrix4f Tbe, Matrix4f Tbe_d) { float eta, eta_d; Matrix3f R = Tbe.block(0,0,3,3); //Matrix.slice<RowStart, ColStart, NumRows, NumCols>(); Vector3f epsilon = r2quat(R, eta); Matrix3f R_d = Tbe_d.block(0,0,3,3); Vector3f epsilon_d = r2quat(R_d, eta_d); Matrix3f S = skew(epsilon_d); Vector3f eo = eta*epsilon_d-eta_d*epsilon-S*epsilon; return eo; } inline Vector3f versorError(Vector3f P_d, Matrix3f Tbe_e, float &theta) { Vector3f P_e = Tbe_e.block(0,3,3,1); Vector3f u_e = Tbe_e.block(0,0,3,1); Vector3f u_d = P_d - P_e; u_d = u_d/ u_d.norm(); Vector3f r = skew(u_e)*u_d; float nr = r.norm(); if (nr >0){ r = r/r.norm(); //be carefult to acos( > 1 ) float u_e_d = u_e.transpose()*u_d; if( fabs(u_e_d) <= 1.0 ) { theta = acos(u_e.transpose()*u_d); } else { theta = 0.0; } Vector3f error = r*sin(theta); return error; }else{ theta = 0.0; return Vector3f::Zero(); } } /*Matrix3f utilities::rotation ( float theta, Vector3f r ) { Matrix3f R = Zeros; R[0][0] = r[0]*r[0]*(1-cos(theta)) + cos(theta); R[1][1] = r[1]*r[1]*(1-cos(theta)) + cos(theta); R[2][2] = r[2]*r[2]*(1-cos(theta)) + cos(theta); R[0][1] = r[0]*r[1]*(1-cos(theta)) - r[2]*sin(theta); R[1][0] = r[0]*r[1]*(1-cos(theta)) + r[2]*sin(theta); R[0][2] = r[0]*r[2]*(1-cos(theta)) + r[1]*sin(theta); R[2][0] = r[0]*r[2]*(1-cos(theta)) - r[1]*sin(theta); R[1][2] = r[1]*r[2]*(1-cos(theta)) - r[0]*sin(theta); R[2][1] = r[1]*r[2]*(1-cos(theta)) + r[0]*sin(theta); return R; }*/ inline Matrix3f XYZ2R(Vector3f angles) { Matrix3f R = Matrix3f::Zero(); Matrix3f R1 = Matrix3f::Zero(); Matrix3f R2 = Matrix3f::Zero(); Matrix3f R3 = Matrix3f::Zero(); float cos_phi = cos(angles[0]); float sin_phi = sin(angles[0]); float cos_theta = cos(angles[1]); float sin_theta = sin(angles[1]); float cos_psi = cos(angles[2]); float sin_psi = sin(angles[2]); R1 << 1, 0 , 0, 0, cos_phi, -sin_phi, 0, sin_phi, cos_phi; R2 << cos_theta , 0, sin_theta, 0 , 1, 0 , -sin_theta, 0, cos_theta; R3 << cos_psi, -sin_psi, 0, sin_psi, cos_psi , 0, 0 , 0 , 1; R = R1*R2*R3; return R; } // This method computes the XYZ Euler angles from the Rotational matrix R. inline Vector3f R2XYZ(Matrix3f R) { float phi=0.0, theta=0.0, psi=0.0; Vector3f XYZ = Vector3f::Zero(); theta = asin(R(0,2)); if(fabsf(cos(theta))>pow(10.0,-10.0)) { phi=atan2(-R(1,2)/cos(theta), R(2,2)/cos(theta)); psi=atan2(-R(0,1)/cos(theta), R(0,0)/cos(theta)); } else { if(fabsf(theta-M_PI/2.0)<pow(10.0,-5.0)) { psi = 0.0; phi = atan2(R(1,0), R(2,0)); theta = M_PI/2.0; } else { psi = 0.0; phi = atan2(-R(1,0), R(2,0)); theta = -M_PI/2.0; } } XYZ << phi,theta,psi; return XYZ; } inline Matrix3f angleAxis2Rot(Vector3f ri, float theta){ Matrix3f R; 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), 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), 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); return R; } inline Vector3f butt_filter(Vector3f x, Vector3f x1, Vector3f x2, float omega_n, float zita, float ctrl_T){ //applico un filtro di Butterworth del secondo ordine (sfrutto Eulero all'indietro) 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); } /// converts a rate in Hz to an integer period in ms. inline uint16_t rateToPeriod(const float & rate) { if (rate > 0) return static_cast<uint16_t> (1000.0 / rate); else return 0; } //Quaternion to rotration Matrix inline Matrix3f QuatToMat(Vector4f Quat){ Matrix3f Rot; float s = Quat[0]; float x = Quat[1]; float y = Quat[2]; float z = Quat[3]; Rot << 1-2*(y*y+z*z),2*(x*y-s*z),2*(x*z+s*y), 2*(x*y+s*z),1-2*(x*x+z*z),2*(y*z-s*x), 2*(x*z-s*y),2*(y*z+s*x),1-2*(x*x+y*y); return Rot; } inline float rad2deg(float rad){ float deg; deg = 180.0*rad/M_PI; return deg; } inline float deg2rad(float deg){ float rad; rad = M_PI*deg/180.0; return rad; } } #endif