Marco De Silva / ARM

a

Dependencies:   MX64 ARMControl

utilities.hpp@2:8d9a79e18fb9, 2021-10-04 (annotated)

Committer:
marcodesilva
Date:
Mon Oct 04 13:36:53 2021 +0000
Revision:
2:8d9a79e18fb9
Parent:
0:46ef60040f6a 
a

Who changed what in which revision?

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