prima prova bracctio
Dependencies: Eigen MX64_senzaCorrente
ARM/utilities.hpp@0:82a5f03b7eb4, 2019-11-18 (annotated)
- Committer:
- anfontanelli
- Date:
- Mon Nov 18 17:50:32 2019 +0000
- Revision:
- 0:82a5f03b7eb4
arm created
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
anfontanelli | 0:82a5f03b7eb4 | 1 | #ifndef _utilities_hpp |
anfontanelli | 0:82a5f03b7eb4 | 2 | #define _utilities_hpp |
anfontanelli | 0:82a5f03b7eb4 | 3 | |
anfontanelli | 0:82a5f03b7eb4 | 4 | |
anfontanelli | 0:82a5f03b7eb4 | 5 | #include "Core.h" |
anfontanelli | 0:82a5f03b7eb4 | 6 | #ifndef M_PI |
anfontanelli | 0:82a5f03b7eb4 | 7 | #define M_PI 3.14159265358979 |
anfontanelli | 0:82a5f03b7eb4 | 8 | #endif |
anfontanelli | 0:82a5f03b7eb4 | 9 | |
anfontanelli | 0:82a5f03b7eb4 | 10 | using namespace std; //calling the standard directory |
anfontanelli | 0:82a5f03b7eb4 | 11 | using namespace Eigen; |
anfontanelli | 0:82a5f03b7eb4 | 12 | |
anfontanelli | 0:82a5f03b7eb4 | 13 | namespace utilities{ |
anfontanelli | 0:82a5f03b7eb4 | 14 | |
anfontanelli | 0:82a5f03b7eb4 | 15 | inline Matrix3f rotx(float alpha){ |
anfontanelli | 0:82a5f03b7eb4 | 16 | Matrix3f Rx; |
anfontanelli | 0:82a5f03b7eb4 | 17 | Rx << 1,0,0, |
anfontanelli | 0:82a5f03b7eb4 | 18 | 0,cos(alpha),-sin(alpha), |
anfontanelli | 0:82a5f03b7eb4 | 19 | 0,sin(alpha), cos(alpha); |
anfontanelli | 0:82a5f03b7eb4 | 20 | return Rx; |
anfontanelli | 0:82a5f03b7eb4 | 21 | } |
anfontanelli | 0:82a5f03b7eb4 | 22 | |
anfontanelli | 0:82a5f03b7eb4 | 23 | inline Matrix3f roty(float beta){ |
anfontanelli | 0:82a5f03b7eb4 | 24 | Matrix3f Ry; |
anfontanelli | 0:82a5f03b7eb4 | 25 | Ry << cos(beta),0,sin(beta), |
anfontanelli | 0:82a5f03b7eb4 | 26 | 0,1,0, |
anfontanelli | 0:82a5f03b7eb4 | 27 | -sin(beta),0, cos(beta); |
anfontanelli | 0:82a5f03b7eb4 | 28 | return Ry; |
anfontanelli | 0:82a5f03b7eb4 | 29 | } |
anfontanelli | 0:82a5f03b7eb4 | 30 | |
anfontanelli | 0:82a5f03b7eb4 | 31 | inline Matrix3f rotz(float gamma){ |
anfontanelli | 0:82a5f03b7eb4 | 32 | Matrix3f Rz; |
anfontanelli | 0:82a5f03b7eb4 | 33 | Rz << cos(gamma),-sin(gamma),0, |
anfontanelli | 0:82a5f03b7eb4 | 34 | sin(gamma),cos(gamma),0, |
anfontanelli | 0:82a5f03b7eb4 | 35 | 0,0, 1; |
anfontanelli | 0:82a5f03b7eb4 | 36 | return Rz; |
anfontanelli | 0:82a5f03b7eb4 | 37 | } |
anfontanelli | 0:82a5f03b7eb4 | 38 | |
anfontanelli | 0:82a5f03b7eb4 | 39 | |
anfontanelli | 0:82a5f03b7eb4 | 40 | inline Matrix4f rotx_T(float alpha){ |
anfontanelli | 0:82a5f03b7eb4 | 41 | Matrix4f Tx = Matrix4f::Identity(); |
anfontanelli | 0:82a5f03b7eb4 | 42 | Tx.block(0,0,3,3) = rotx(alpha); |
anfontanelli | 0:82a5f03b7eb4 | 43 | return Tx; |
anfontanelli | 0:82a5f03b7eb4 | 44 | } |
anfontanelli | 0:82a5f03b7eb4 | 45 | |
anfontanelli | 0:82a5f03b7eb4 | 46 | inline Matrix4f roty_T(float beta){ |
anfontanelli | 0:82a5f03b7eb4 | 47 | Matrix4f Ty = Matrix4f::Identity(); |
anfontanelli | 0:82a5f03b7eb4 | 48 | Ty.block(0,0,3,3) = roty(beta); |
anfontanelli | 0:82a5f03b7eb4 | 49 | return Ty; |
anfontanelli | 0:82a5f03b7eb4 | 50 | } |
anfontanelli | 0:82a5f03b7eb4 | 51 | |
anfontanelli | 0:82a5f03b7eb4 | 52 | inline Matrix4f rotz_T(float gamma){ |
anfontanelli | 0:82a5f03b7eb4 | 53 | Matrix4f Tz = Matrix4f::Identity(); |
anfontanelli | 0:82a5f03b7eb4 | 54 | Tz.block(0,0,3,3) = rotz(gamma); |
anfontanelli | 0:82a5f03b7eb4 | 55 | return Tz; |
anfontanelli | 0:82a5f03b7eb4 | 56 | } |
anfontanelli | 0:82a5f03b7eb4 | 57 | |
anfontanelli | 0:82a5f03b7eb4 | 58 | inline Matrix3f skew(Vector3f v) |
anfontanelli | 0:82a5f03b7eb4 | 59 | { |
anfontanelli | 0:82a5f03b7eb4 | 60 | Matrix3f S; |
anfontanelli | 0:82a5f03b7eb4 | 61 | S << 0, -v[2], v[1], //Skew-symmetric matrix |
anfontanelli | 0:82a5f03b7eb4 | 62 | v[2], 0, -v[0], |
anfontanelli | 0:82a5f03b7eb4 | 63 | -v[1], v[0], 0; |
anfontanelli | 0:82a5f03b7eb4 | 64 | return S; |
anfontanelli | 0:82a5f03b7eb4 | 65 | } |
anfontanelli | 0:82a5f03b7eb4 | 66 | |
anfontanelli | 0:82a5f03b7eb4 | 67 | |
anfontanelli | 0:82a5f03b7eb4 | 68 | inline Matrix3f L_matrix(Matrix3f R_d, Matrix3f R_e) |
anfontanelli | 0:82a5f03b7eb4 | 69 | { |
anfontanelli | 0:82a5f03b7eb4 | 70 | 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))); |
anfontanelli | 0:82a5f03b7eb4 | 71 | return L; |
anfontanelli | 0:82a5f03b7eb4 | 72 | } |
anfontanelli | 0:82a5f03b7eb4 | 73 | |
anfontanelli | 0:82a5f03b7eb4 | 74 | |
anfontanelli | 0:82a5f03b7eb4 | 75 | |
anfontanelli | 0:82a5f03b7eb4 | 76 | inline Vector3f rotationMatrixError(Matrix4f Td, Matrix4f Te) |
anfontanelli | 0:82a5f03b7eb4 | 77 | { |
anfontanelli | 0:82a5f03b7eb4 | 78 | |
anfontanelli | 0:82a5f03b7eb4 | 79 | Matrix3f R_e = Te.block(0,0,3,3); //Matrix.slice<RowStart, ColStart, NumRows, NumCols>(); |
anfontanelli | 0:82a5f03b7eb4 | 80 | Matrix3f R_d = Td.block(0,0,3,3); |
anfontanelli | 0:82a5f03b7eb4 | 81 | |
anfontanelli | 0:82a5f03b7eb4 | 82 | 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)) ; |
anfontanelli | 0:82a5f03b7eb4 | 83 | return eo; |
anfontanelli | 0:82a5f03b7eb4 | 84 | } |
anfontanelli | 0:82a5f03b7eb4 | 85 | |
anfontanelli | 0:82a5f03b7eb4 | 86 | |
anfontanelli | 0:82a5f03b7eb4 | 87 | inline Vector3f r2quat(Matrix3f R_iniz, float &eta) |
anfontanelli | 0:82a5f03b7eb4 | 88 | { |
anfontanelli | 0:82a5f03b7eb4 | 89 | Vector3f epsilon; |
anfontanelli | 0:82a5f03b7eb4 | 90 | int iu, iv, iw; |
anfontanelli | 0:82a5f03b7eb4 | 91 | |
anfontanelli | 0:82a5f03b7eb4 | 92 | if ( (R_iniz(0,0) >= R_iniz(1,1)) && (R_iniz(0,0) >= R_iniz(2,2)) ) |
anfontanelli | 0:82a5f03b7eb4 | 93 | { |
anfontanelli | 0:82a5f03b7eb4 | 94 | iu = 0; iv = 1; iw = 2; |
anfontanelli | 0:82a5f03b7eb4 | 95 | } |
anfontanelli | 0:82a5f03b7eb4 | 96 | else if ( (R_iniz(1,1) >= R_iniz(0,0)) && (R_iniz(1,1) >= R_iniz(2,2)) ) |
anfontanelli | 0:82a5f03b7eb4 | 97 | { |
anfontanelli | 0:82a5f03b7eb4 | 98 | iu = 1; iv = 2; iw = 0; |
anfontanelli | 0:82a5f03b7eb4 | 99 | } |
anfontanelli | 0:82a5f03b7eb4 | 100 | else |
anfontanelli | 0:82a5f03b7eb4 | 101 | { |
anfontanelli | 0:82a5f03b7eb4 | 102 | iu = 2; iv = 0; iw = 1; |
anfontanelli | 0:82a5f03b7eb4 | 103 | } |
anfontanelli | 0:82a5f03b7eb4 | 104 | |
anfontanelli | 0:82a5f03b7eb4 | 105 | float r = sqrt(1 + R_iniz(iu,iu) - R_iniz(iv,iv) - R_iniz(iw,iw)); |
anfontanelli | 0:82a5f03b7eb4 | 106 | Vector3f q; |
anfontanelli | 0:82a5f03b7eb4 | 107 | q << 0,0,0; |
anfontanelli | 0:82a5f03b7eb4 | 108 | if (r>0) |
anfontanelli | 0:82a5f03b7eb4 | 109 | { |
anfontanelli | 0:82a5f03b7eb4 | 110 | float rr = 2*r; |
anfontanelli | 0:82a5f03b7eb4 | 111 | eta = (R_iniz(iw,iv)-R_iniz(iv,iw)/rr); |
anfontanelli | 0:82a5f03b7eb4 | 112 | epsilon[iu] = r/2; |
anfontanelli | 0:82a5f03b7eb4 | 113 | epsilon[iv] = (R_iniz(iu,iv)+R_iniz(iv,iu))/rr; |
anfontanelli | 0:82a5f03b7eb4 | 114 | epsilon[iw] = (R_iniz(iw,iu)+R_iniz(iu,iw))/rr; |
anfontanelli | 0:82a5f03b7eb4 | 115 | } |
anfontanelli | 0:82a5f03b7eb4 | 116 | else |
anfontanelli | 0:82a5f03b7eb4 | 117 | { |
anfontanelli | 0:82a5f03b7eb4 | 118 | eta = 1; |
anfontanelli | 0:82a5f03b7eb4 | 119 | epsilon << 0,0,0; |
anfontanelli | 0:82a5f03b7eb4 | 120 | } |
anfontanelli | 0:82a5f03b7eb4 | 121 | return epsilon; |
anfontanelli | 0:82a5f03b7eb4 | 122 | } |
anfontanelli | 0:82a5f03b7eb4 | 123 | |
anfontanelli | 0:82a5f03b7eb4 | 124 | |
anfontanelli | 0:82a5f03b7eb4 | 125 | inline Vector4f rot2quat(Matrix3f R){ |
anfontanelli | 0:82a5f03b7eb4 | 126 | |
anfontanelli | 0:82a5f03b7eb4 | 127 | float m00, m01, m02, m10, m11, m12, m20, m21, m22; |
anfontanelli | 0:82a5f03b7eb4 | 128 | |
anfontanelli | 0:82a5f03b7eb4 | 129 | m00 = R(0,0); |
anfontanelli | 0:82a5f03b7eb4 | 130 | m01 = R(0,1); |
anfontanelli | 0:82a5f03b7eb4 | 131 | m02 = R(0,2); |
anfontanelli | 0:82a5f03b7eb4 | 132 | m10 = R(1,0); |
anfontanelli | 0:82a5f03b7eb4 | 133 | m11 = R(1,1); |
anfontanelli | 0:82a5f03b7eb4 | 134 | m12 = R(1,2); |
anfontanelli | 0:82a5f03b7eb4 | 135 | m20 = R(2,0); |
anfontanelli | 0:82a5f03b7eb4 | 136 | m21 = R(2,1); |
anfontanelli | 0:82a5f03b7eb4 | 137 | m22 = R(2,2); |
anfontanelli | 0:82a5f03b7eb4 | 138 | |
anfontanelli | 0:82a5f03b7eb4 | 139 | float tr = m00 + m11 + m22; |
anfontanelli | 0:82a5f03b7eb4 | 140 | float qw, qx, qy, qz, S; |
anfontanelli | 0:82a5f03b7eb4 | 141 | Vector4f quat; |
anfontanelli | 0:82a5f03b7eb4 | 142 | |
anfontanelli | 0:82a5f03b7eb4 | 143 | if (tr > 0) { |
anfontanelli | 0:82a5f03b7eb4 | 144 | S = sqrt(tr+1.0) * 2; // S=4*qw |
anfontanelli | 0:82a5f03b7eb4 | 145 | qw = 0.25 * S; |
anfontanelli | 0:82a5f03b7eb4 | 146 | qx = (m21 - m12) / S; |
anfontanelli | 0:82a5f03b7eb4 | 147 | qy = (m02 - m20) / S; |
anfontanelli | 0:82a5f03b7eb4 | 148 | qz = (m10 - m01) / S; |
anfontanelli | 0:82a5f03b7eb4 | 149 | } else if ((m00 > m11)&(m00 > m22)) { |
anfontanelli | 0:82a5f03b7eb4 | 150 | S = sqrt(1.0 + m00 - m11 - m22) * 2; // S=4*qx |
anfontanelli | 0:82a5f03b7eb4 | 151 | qw = (m21 - m12) / S; |
anfontanelli | 0:82a5f03b7eb4 | 152 | qx = 0.25 * S; |
anfontanelli | 0:82a5f03b7eb4 | 153 | qy = (m01 + m10) / S; |
anfontanelli | 0:82a5f03b7eb4 | 154 | qz = (m02 + m20) / S; |
anfontanelli | 0:82a5f03b7eb4 | 155 | } else if (m11 > m22) { |
anfontanelli | 0:82a5f03b7eb4 | 156 | S = sqrt(1.0 + m11 - m00 - m22) * 2; // S=4*qy |
anfontanelli | 0:82a5f03b7eb4 | 157 | qw = (m02 - m20) / S; |
anfontanelli | 0:82a5f03b7eb4 | 158 | qx = (m01 + m10) / S; |
anfontanelli | 0:82a5f03b7eb4 | 159 | qy = 0.25 * S; |
anfontanelli | 0:82a5f03b7eb4 | 160 | qz = (m12 + m21) / S; |
anfontanelli | 0:82a5f03b7eb4 | 161 | } else { |
anfontanelli | 0:82a5f03b7eb4 | 162 | S = sqrt(1.0 + m22 - m00 - m11) * 2; // S=4*qz |
anfontanelli | 0:82a5f03b7eb4 | 163 | qw = (m10 - m01) / S; |
anfontanelli | 0:82a5f03b7eb4 | 164 | qx = (m02 + m20) / S; |
anfontanelli | 0:82a5f03b7eb4 | 165 | qy = (m12 + m21) / S; |
anfontanelli | 0:82a5f03b7eb4 | 166 | qz = 0.25 * S; |
anfontanelli | 0:82a5f03b7eb4 | 167 | } |
anfontanelli | 0:82a5f03b7eb4 | 168 | |
anfontanelli | 0:82a5f03b7eb4 | 169 | quat << qw, qx, qy, qz; |
anfontanelli | 0:82a5f03b7eb4 | 170 | return quat; |
anfontanelli | 0:82a5f03b7eb4 | 171 | |
anfontanelli | 0:82a5f03b7eb4 | 172 | } |
anfontanelli | 0:82a5f03b7eb4 | 173 | |
anfontanelli | 0:82a5f03b7eb4 | 174 | |
anfontanelli | 0:82a5f03b7eb4 | 175 | //Matrix ortonormalization |
anfontanelli | 0:82a5f03b7eb4 | 176 | inline Matrix3f matrixOrthonormalization(Matrix3f R){ |
anfontanelli | 0:82a5f03b7eb4 | 177 | |
anfontanelli | 0:82a5f03b7eb4 | 178 | SelfAdjointEigenSolver<Matrix3f> es(R.transpose()*R); |
anfontanelli | 0:82a5f03b7eb4 | 179 | Vector3f D = es.eigenvalues(); |
anfontanelli | 0:82a5f03b7eb4 | 180 | Matrix3f V = es.eigenvectors(); |
anfontanelli | 0:82a5f03b7eb4 | 181 | 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()); |
anfontanelli | 0:82a5f03b7eb4 | 182 | |
anfontanelli | 0:82a5f03b7eb4 | 183 | return R; |
anfontanelli | 0:82a5f03b7eb4 | 184 | } |
anfontanelli | 0:82a5f03b7eb4 | 185 | |
anfontanelli | 0:82a5f03b7eb4 | 186 | |
anfontanelli | 0:82a5f03b7eb4 | 187 | |
anfontanelli | 0:82a5f03b7eb4 | 188 | |
anfontanelli | 0:82a5f03b7eb4 | 189 | //****************************************************************************** |
anfontanelli | 0:82a5f03b7eb4 | 190 | inline Vector3f quaternionError(Matrix4f Tbe, Matrix4f Tbe_d) |
anfontanelli | 0:82a5f03b7eb4 | 191 | { |
anfontanelli | 0:82a5f03b7eb4 | 192 | float eta, eta_d; |
anfontanelli | 0:82a5f03b7eb4 | 193 | Matrix3f R = Tbe.block(0,0,3,3); //Matrix.slice<RowStart, ColStart, NumRows, NumCols>(); |
anfontanelli | 0:82a5f03b7eb4 | 194 | Vector3f epsilon = r2quat(R, eta); |
anfontanelli | 0:82a5f03b7eb4 | 195 | Matrix3f R_d = Tbe_d.block(0,0,3,3); |
anfontanelli | 0:82a5f03b7eb4 | 196 | Vector3f epsilon_d = r2quat(R_d, eta_d); |
anfontanelli | 0:82a5f03b7eb4 | 197 | Matrix3f S = skew(epsilon_d); |
anfontanelli | 0:82a5f03b7eb4 | 198 | Vector3f eo = eta*epsilon_d-eta_d*epsilon-S*epsilon; |
anfontanelli | 0:82a5f03b7eb4 | 199 | return eo; |
anfontanelli | 0:82a5f03b7eb4 | 200 | } |
anfontanelli | 0:82a5f03b7eb4 | 201 | |
anfontanelli | 0:82a5f03b7eb4 | 202 | |
anfontanelli | 0:82a5f03b7eb4 | 203 | inline Vector3f versorError(Vector3f P_d, Matrix3f Tbe_e, float &theta) |
anfontanelli | 0:82a5f03b7eb4 | 204 | { |
anfontanelli | 0:82a5f03b7eb4 | 205 | Vector3f P_e = Tbe_e.block(0,3,3,1); |
anfontanelli | 0:82a5f03b7eb4 | 206 | Vector3f u_e = Tbe_e.block(0,0,3,1); |
anfontanelli | 0:82a5f03b7eb4 | 207 | |
anfontanelli | 0:82a5f03b7eb4 | 208 | Vector3f u_d = P_d - P_e; |
anfontanelli | 0:82a5f03b7eb4 | 209 | u_d = u_d/ u_d.norm(); |
anfontanelli | 0:82a5f03b7eb4 | 210 | |
anfontanelli | 0:82a5f03b7eb4 | 211 | Vector3f r = skew(u_e)*u_d; |
anfontanelli | 0:82a5f03b7eb4 | 212 | float nr = r.norm(); |
anfontanelli | 0:82a5f03b7eb4 | 213 | |
anfontanelli | 0:82a5f03b7eb4 | 214 | if (nr >0){ |
anfontanelli | 0:82a5f03b7eb4 | 215 | r = r/r.norm(); |
anfontanelli | 0:82a5f03b7eb4 | 216 | |
anfontanelli | 0:82a5f03b7eb4 | 217 | |
anfontanelli | 0:82a5f03b7eb4 | 218 | //be carefult to acos( > 1 ) |
anfontanelli | 0:82a5f03b7eb4 | 219 | float u_e_d = u_e.transpose()*u_d; |
anfontanelli | 0:82a5f03b7eb4 | 220 | if( fabs(u_e_d) <= 1.0 ) { |
anfontanelli | 0:82a5f03b7eb4 | 221 | theta = acos(u_e.transpose()*u_d); |
anfontanelli | 0:82a5f03b7eb4 | 222 | } |
anfontanelli | 0:82a5f03b7eb4 | 223 | else { |
anfontanelli | 0:82a5f03b7eb4 | 224 | theta = 0.0; |
anfontanelli | 0:82a5f03b7eb4 | 225 | } |
anfontanelli | 0:82a5f03b7eb4 | 226 | |
anfontanelli | 0:82a5f03b7eb4 | 227 | Vector3f error = r*sin(theta); |
anfontanelli | 0:82a5f03b7eb4 | 228 | return error; |
anfontanelli | 0:82a5f03b7eb4 | 229 | }else{ |
anfontanelli | 0:82a5f03b7eb4 | 230 | theta = 0.0; |
anfontanelli | 0:82a5f03b7eb4 | 231 | return Vector3f::Zero(); |
anfontanelli | 0:82a5f03b7eb4 | 232 | } |
anfontanelli | 0:82a5f03b7eb4 | 233 | } |
anfontanelli | 0:82a5f03b7eb4 | 234 | |
anfontanelli | 0:82a5f03b7eb4 | 235 | |
anfontanelli | 0:82a5f03b7eb4 | 236 | /*Matrix3f utilities::rotation ( float theta, Vector3f r ) { |
anfontanelli | 0:82a5f03b7eb4 | 237 | |
anfontanelli | 0:82a5f03b7eb4 | 238 | Matrix3f R = Zeros; |
anfontanelli | 0:82a5f03b7eb4 | 239 | |
anfontanelli | 0:82a5f03b7eb4 | 240 | R[0][0] = r[0]*r[0]*(1-cos(theta)) + cos(theta); |
anfontanelli | 0:82a5f03b7eb4 | 241 | R[1][1] = r[1]*r[1]*(1-cos(theta)) + cos(theta); |
anfontanelli | 0:82a5f03b7eb4 | 242 | R[2][2] = r[2]*r[2]*(1-cos(theta)) + cos(theta); |
anfontanelli | 0:82a5f03b7eb4 | 243 | |
anfontanelli | 0:82a5f03b7eb4 | 244 | R[0][1] = r[0]*r[1]*(1-cos(theta)) - r[2]*sin(theta); |
anfontanelli | 0:82a5f03b7eb4 | 245 | R[1][0] = r[0]*r[1]*(1-cos(theta)) + r[2]*sin(theta); |
anfontanelli | 0:82a5f03b7eb4 | 246 | |
anfontanelli | 0:82a5f03b7eb4 | 247 | R[0][2] = r[0]*r[2]*(1-cos(theta)) + r[1]*sin(theta); |
anfontanelli | 0:82a5f03b7eb4 | 248 | R[2][0] = r[0]*r[2]*(1-cos(theta)) - r[1]*sin(theta); |
anfontanelli | 0:82a5f03b7eb4 | 249 | |
anfontanelli | 0:82a5f03b7eb4 | 250 | R[1][2] = r[1]*r[2]*(1-cos(theta)) - r[0]*sin(theta); |
anfontanelli | 0:82a5f03b7eb4 | 251 | R[2][1] = r[1]*r[2]*(1-cos(theta)) + r[0]*sin(theta); |
anfontanelli | 0:82a5f03b7eb4 | 252 | |
anfontanelli | 0:82a5f03b7eb4 | 253 | |
anfontanelli | 0:82a5f03b7eb4 | 254 | return R; |
anfontanelli | 0:82a5f03b7eb4 | 255 | }*/ |
anfontanelli | 0:82a5f03b7eb4 | 256 | |
anfontanelli | 0:82a5f03b7eb4 | 257 | |
anfontanelli | 0:82a5f03b7eb4 | 258 | inline Matrix3f XYZ2R(Vector3f angles) { |
anfontanelli | 0:82a5f03b7eb4 | 259 | |
anfontanelli | 0:82a5f03b7eb4 | 260 | Matrix3f R = Matrix3f::Zero(); |
anfontanelli | 0:82a5f03b7eb4 | 261 | Matrix3f R1 = Matrix3f::Zero(); |
anfontanelli | 0:82a5f03b7eb4 | 262 | Matrix3f R2 = Matrix3f::Zero(); |
anfontanelli | 0:82a5f03b7eb4 | 263 | Matrix3f R3 = Matrix3f::Zero(); |
anfontanelli | 0:82a5f03b7eb4 | 264 | |
anfontanelli | 0:82a5f03b7eb4 | 265 | float cos_phi = cos(angles[0]); |
anfontanelli | 0:82a5f03b7eb4 | 266 | float sin_phi = sin(angles[0]); |
anfontanelli | 0:82a5f03b7eb4 | 267 | float cos_theta = cos(angles[1]); |
anfontanelli | 0:82a5f03b7eb4 | 268 | float sin_theta = sin(angles[1]); |
anfontanelli | 0:82a5f03b7eb4 | 269 | float cos_psi = cos(angles[2]); |
anfontanelli | 0:82a5f03b7eb4 | 270 | float sin_psi = sin(angles[2]); |
anfontanelli | 0:82a5f03b7eb4 | 271 | |
anfontanelli | 0:82a5f03b7eb4 | 272 | R1 << 1, 0 , 0, |
anfontanelli | 0:82a5f03b7eb4 | 273 | 0, cos_phi, -sin_phi, |
anfontanelli | 0:82a5f03b7eb4 | 274 | 0, sin_phi, cos_phi; |
anfontanelli | 0:82a5f03b7eb4 | 275 | |
anfontanelli | 0:82a5f03b7eb4 | 276 | R2 << cos_theta , 0, sin_theta, |
anfontanelli | 0:82a5f03b7eb4 | 277 | 0 , 1, 0 , |
anfontanelli | 0:82a5f03b7eb4 | 278 | -sin_theta, 0, cos_theta; |
anfontanelli | 0:82a5f03b7eb4 | 279 | |
anfontanelli | 0:82a5f03b7eb4 | 280 | R3 << cos_psi, -sin_psi, 0, |
anfontanelli | 0:82a5f03b7eb4 | 281 | sin_psi, cos_psi , 0, |
anfontanelli | 0:82a5f03b7eb4 | 282 | 0 , 0 , 1; |
anfontanelli | 0:82a5f03b7eb4 | 283 | |
anfontanelli | 0:82a5f03b7eb4 | 284 | R = R1*R2*R3; |
anfontanelli | 0:82a5f03b7eb4 | 285 | |
anfontanelli | 0:82a5f03b7eb4 | 286 | return R; |
anfontanelli | 0:82a5f03b7eb4 | 287 | } |
anfontanelli | 0:82a5f03b7eb4 | 288 | |
anfontanelli | 0:82a5f03b7eb4 | 289 | // This method computes the XYZ Euler angles from the Rotational matrix R. |
anfontanelli | 0:82a5f03b7eb4 | 290 | inline Vector3f R2XYZ(Matrix3f R) { |
anfontanelli | 0:82a5f03b7eb4 | 291 | float phi=0.0, theta=0.0, psi=0.0; |
anfontanelli | 0:82a5f03b7eb4 | 292 | Vector3f XYZ = Vector3f::Zero(); |
anfontanelli | 0:82a5f03b7eb4 | 293 | |
anfontanelli | 0:82a5f03b7eb4 | 294 | theta = asin(R(0,2)); |
anfontanelli | 0:82a5f03b7eb4 | 295 | |
anfontanelli | 0:82a5f03b7eb4 | 296 | if(fabsf(cos(theta))>pow(10.0,-10.0)) |
anfontanelli | 0:82a5f03b7eb4 | 297 | { |
anfontanelli | 0:82a5f03b7eb4 | 298 | phi=atan2(-R(1,2)/cos(theta), R(2,2)/cos(theta)); |
anfontanelli | 0:82a5f03b7eb4 | 299 | psi=atan2(-R(0,1)/cos(theta), R(0,0)/cos(theta)); |
anfontanelli | 0:82a5f03b7eb4 | 300 | } |
anfontanelli | 0:82a5f03b7eb4 | 301 | else |
anfontanelli | 0:82a5f03b7eb4 | 302 | { |
anfontanelli | 0:82a5f03b7eb4 | 303 | if(fabsf(theta-M_PI/2.0)<pow(10.0,-5.0)) |
anfontanelli | 0:82a5f03b7eb4 | 304 | { |
anfontanelli | 0:82a5f03b7eb4 | 305 | psi = 0.0; |
anfontanelli | 0:82a5f03b7eb4 | 306 | phi = atan2(R(1,0), R(2,0)); |
anfontanelli | 0:82a5f03b7eb4 | 307 | theta = M_PI/2.0; |
anfontanelli | 0:82a5f03b7eb4 | 308 | } |
anfontanelli | 0:82a5f03b7eb4 | 309 | else |
anfontanelli | 0:82a5f03b7eb4 | 310 | { |
anfontanelli | 0:82a5f03b7eb4 | 311 | psi = 0.0; |
anfontanelli | 0:82a5f03b7eb4 | 312 | phi = atan2(-R(1,0), R(2,0)); |
anfontanelli | 0:82a5f03b7eb4 | 313 | theta = -M_PI/2.0; |
anfontanelli | 0:82a5f03b7eb4 | 314 | } |
anfontanelli | 0:82a5f03b7eb4 | 315 | } |
anfontanelli | 0:82a5f03b7eb4 | 316 | |
anfontanelli | 0:82a5f03b7eb4 | 317 | XYZ << phi,theta,psi; |
anfontanelli | 0:82a5f03b7eb4 | 318 | return XYZ; |
anfontanelli | 0:82a5f03b7eb4 | 319 | } |
anfontanelli | 0:82a5f03b7eb4 | 320 | |
anfontanelli | 0:82a5f03b7eb4 | 321 | inline Matrix3f angleAxis2Rot(Vector3f ri, float theta){ |
anfontanelli | 0:82a5f03b7eb4 | 322 | Matrix3f R; |
anfontanelli | 0:82a5f03b7eb4 | 323 | 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), |
anfontanelli | 0:82a5f03b7eb4 | 324 | 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), |
anfontanelli | 0:82a5f03b7eb4 | 325 | 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); |
anfontanelli | 0:82a5f03b7eb4 | 326 | |
anfontanelli | 0:82a5f03b7eb4 | 327 | return R; |
anfontanelli | 0:82a5f03b7eb4 | 328 | |
anfontanelli | 0:82a5f03b7eb4 | 329 | } |
anfontanelli | 0:82a5f03b7eb4 | 330 | |
anfontanelli | 0:82a5f03b7eb4 | 331 | |
anfontanelli | 0:82a5f03b7eb4 | 332 | |
anfontanelli | 0:82a5f03b7eb4 | 333 | inline Vector3f butt_filter(Vector3f x, Vector3f x1, Vector3f x2, float omega_n, float zita, float ctrl_T){ |
anfontanelli | 0:82a5f03b7eb4 | 334 | //applico un filtro di Butterworth del secondo ordine (sfrutto Eulero all'indietro) |
anfontanelli | 0:82a5f03b7eb4 | 335 | 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); |
anfontanelli | 0:82a5f03b7eb4 | 336 | } |
anfontanelli | 0:82a5f03b7eb4 | 337 | |
anfontanelli | 0:82a5f03b7eb4 | 338 | |
anfontanelli | 0:82a5f03b7eb4 | 339 | |
anfontanelli | 0:82a5f03b7eb4 | 340 | /// converts a rate in Hz to an integer period in ms. |
anfontanelli | 0:82a5f03b7eb4 | 341 | inline uint16_t rateToPeriod(const float & rate) { |
anfontanelli | 0:82a5f03b7eb4 | 342 | if (rate > 0) |
anfontanelli | 0:82a5f03b7eb4 | 343 | return static_cast<uint16_t> (1000.0 / rate); |
anfontanelli | 0:82a5f03b7eb4 | 344 | else |
anfontanelli | 0:82a5f03b7eb4 | 345 | return 0; |
anfontanelli | 0:82a5f03b7eb4 | 346 | } |
anfontanelli | 0:82a5f03b7eb4 | 347 | |
anfontanelli | 0:82a5f03b7eb4 | 348 | |
anfontanelli | 0:82a5f03b7eb4 | 349 | |
anfontanelli | 0:82a5f03b7eb4 | 350 | |
anfontanelli | 0:82a5f03b7eb4 | 351 | //Quaternion to rotration Matrix |
anfontanelli | 0:82a5f03b7eb4 | 352 | inline Matrix3f QuatToMat(Vector4f Quat){ |
anfontanelli | 0:82a5f03b7eb4 | 353 | Matrix3f Rot; |
anfontanelli | 0:82a5f03b7eb4 | 354 | float s = Quat[0]; |
anfontanelli | 0:82a5f03b7eb4 | 355 | float x = Quat[1]; |
anfontanelli | 0:82a5f03b7eb4 | 356 | float y = Quat[2]; |
anfontanelli | 0:82a5f03b7eb4 | 357 | float z = Quat[3]; |
anfontanelli | 0:82a5f03b7eb4 | 358 | Rot << 1-2*(y*y+z*z),2*(x*y-s*z),2*(x*z+s*y), |
anfontanelli | 0:82a5f03b7eb4 | 359 | 2*(x*y+s*z),1-2*(x*x+z*z),2*(y*z-s*x), |
anfontanelli | 0:82a5f03b7eb4 | 360 | 2*(x*z-s*y),2*(y*z+s*x),1-2*(x*x+y*y); |
anfontanelli | 0:82a5f03b7eb4 | 361 | return Rot; |
anfontanelli | 0:82a5f03b7eb4 | 362 | } |
anfontanelli | 0:82a5f03b7eb4 | 363 | |
anfontanelli | 0:82a5f03b7eb4 | 364 | |
anfontanelli | 0:82a5f03b7eb4 | 365 | inline float rad2deg(float rad){ |
anfontanelli | 0:82a5f03b7eb4 | 366 | float deg; |
anfontanelli | 0:82a5f03b7eb4 | 367 | deg = 180.0*rad/M_PI; |
anfontanelli | 0:82a5f03b7eb4 | 368 | return deg; |
anfontanelli | 0:82a5f03b7eb4 | 369 | } |
anfontanelli | 0:82a5f03b7eb4 | 370 | |
anfontanelli | 0:82a5f03b7eb4 | 371 | inline float deg2rad(float deg){ |
anfontanelli | 0:82a5f03b7eb4 | 372 | float rad; |
anfontanelli | 0:82a5f03b7eb4 | 373 | rad = M_PI*deg/180.0; |
anfontanelli | 0:82a5f03b7eb4 | 374 | return rad; |
anfontanelli | 0:82a5f03b7eb4 | 375 | } |
anfontanelli | 0:82a5f03b7eb4 | 376 | } |
anfontanelli | 0:82a5f03b7eb4 | 377 | |
anfontanelli | 0:82a5f03b7eb4 | 378 | #endif |
anfontanelli | 0:82a5f03b7eb4 | 379 |