PRISMA Lab / Mbed OS Hyfliers_arm_2

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?

UserRevisionLine numberNew 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