// // Academic License - for use in teaching, academic research, and meeting // course requirements at degree granting institutions only. Not for // government, commercial, or other organizational use. // // get_Kinematics.cpp // // Code generation for function 'get_Kinematics' // // Include files #include "get_Kinematics.h" #include "MatrixExp6.h" #include "calculateGravityForce.h" #include "calculateGravityForce_data.h" #include "rt_nonfinite.h" #include // Variable Definitions static emlrtRTEInfo emlrtRTEI = { 11, // lineNo 25, // colNo "get_Kinematics", // fName "C:\\R1000 EVT GravityForce V1\\get_Kinematics.m"// pName }; static emlrtBCInfo emlrtBCI = { 1, // iFirst 9, // iLast 12, // lineNo 69, // colNo "thetalist", // aName "get_Kinematics", // fName "C:\\R1000 EVT GravityForce V1\\get_Kinematics.m",// pName 0 // checkKind }; // Function Definitions void get_Kinematics(const emlrtStack *sp, real_T *robot_ndof, real_T robot_m[9], real_T robot_I[81], real_T robot_com[27], real_T robot_axis [27], real_T robot_com_pos_R1[27], real_T robot_com_pos_R2 [27], real_T robot_slist[54], real_T robot_theta[9], real_T robot_dtheta[9], real_T robot_ddtheta[9], real_T robot_Home_com[27], real_T robot_Home_R[81], real_T robot_Home_P[27], real_T robot_Home_M[144], struct_T *robot_kine, real_T robot_gravity[3]) { static const real_T dv[9] = { 21.31526, 19.235354, 10.463871, 6.1538806, 3.0882893, 2.4450941, 3.6376589, 1.700542, 0.91507819 }; static const real_T dv1[81] = { 0.11212091, -0.011694021, -0.047186593, -0.011694021, 0.23289532, -0.00030395414, -0.047186593, -0.00030395414, 0.20033874999999998, 0.037926328, -9.0569033E-5, 0.047526574999999995, -9.0569033E-5, 0.29714754, 6.8396714999999995E-6, 0.047526574999999995, 6.8396714999999995E-6, 0.28138391999999995, 0.0044513887, 1.9981964E-7, -0.00030303891, 1.9981964E-7, 0.067952039, -8.8585863999999993E-8, -0.00030303891, -8.8585863999999993E-8, 0.069958343999999992, 0.011642351, 0.0022997175, 0.0029159431, 0.0022997175, 0.026031269, -0.00013518384, 0.0029159431, -0.00013518384, 0.024694742, 0.0030930543999999997, 8.3558814E-7, -0.0028169092, 8.3558814E-7, 0.012796446, -3.3666469E-6, -0.0028169092, -3.3666469E-6, 0.012128855999999999, 0.0036635776, -7.0081461E-6, 2.2392869999999997E-6, -7.0081461E-6, 0.0018152304999999999, -0.00024828765, 2.2392869999999997E-6, -0.00024828765, 0.0034602935, 0.013662651999999999, -3.6340952999999995E-6, 4.401167E-7, -3.6340952999999995E-6, 0.013222824, -0.000436255, 4.401167E-7, -0.000436255, 0.0022500397, 0.0015162872, -4.6245133E-5, 0.0044556929, -4.6245133E-5, 0.059901605999999996, 6.0350547999999996E-6, 0.0044556929, 6.0350547999999996E-6, 0.058819998, 0.0011730106, 3.3834506000000002E-6, 4.6097678E-5, 3.3834506000000002E-6, 0.0017996851999999997, 5.2088777999999993E-6, 4.6097678E-5, 5.2088777999999993E-6, 0.0013960734 }; static const real_T dv2[27] = { 0.10440117, -0.011125559000000002, 0.12822933, 0.35562148000000005, 0.012378198, 1.839583E-6, 0.24798242, -0.035267916, 4.8648267E-5, 0.042341068999999995, 0.052603527000000004, 0.012271365, 0.10177553, -3.4868152E-5, 0.00156861, 0.059942347, -4.8346177E-5, 0.0055784018000000005, 4.9494391999999996E-5, -0.0024240394, -0.10332489, -0.027158498000000003, -0.00033865875, -0.046588829, -0.019853197000000003, 0.00019076829000000002, -0.030075564 }; static const real_T dv3[27] = { -0.08059883000000001, -0.042225558999999996, -0.015870672000000002, -0.24437852000000002, 0.094778198000000008, 1.839583E-6, -0.35201758, -0.029007916, 4.8648266000000007E-5, -0.077658931, -0.012956223, 0.019271365, -0.10372447, -3.9239008E-5, -0.0054313602000000006, -0.023757653, -4.8346177E-5, 0.040028402, 4.9494391999999996E-5, -0.0024240394, 0.27222511, 0.22423433, -0.00033865729, 0.019671171, 0.0, 0.0, 0.0 }; static const real_T dv4[54] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.144100002, 0.0, -0.185, 0.0, -1.0, 0.0, 0.144100002, 0.0, -0.785, 0.0, -1.0, 0.0, 0.14410000200099998, 0.0, -1.385, 0.0, 0.0, 1.0, 0.0079997499999999826, -1.505, 0.0, 1.0, 0.0, 0.0, 0.0, 0.144099972201, -0.0080041208559999827, 0.0, 0.0, -1.0, -0.0080041208559999827, 1.7942, 0.0, 0.0, -1.0, 0.0, -0.26590002799900003, -0.0, -1.7942, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0 }; static const real_T dv5[27] = { 0.0, 0.0, 0.0, 0.185, 0.031099999999999996, 0.144100002, 0.785, -0.051300000000000012, 0.144100002, 1.385, -0.057560000000000021, 0.14410000200099998, 1.505, 0.0079997499999999826, 0.13710000200099998, 1.7105, 0.0080041208559999827, 0.144099972201, 1.7942, 0.0080041208559999827, 0.10964997200099999, 1.7942, 0.0080041208559999827, -0.26590002799900003, 1.542807172, 0.0080041193959999833, -0.332160027999 }; static const real_T dv6[144] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.185, 0.031099999999999996, 0.144100002, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.785, -0.051300000000000012, 0.144100002, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, -1.0, 0.0, 0.0, 1.385, -0.057560000000000021, 0.14410000200099998, 1.0, 0.0, -1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1.505, 0.0079997499999999826, 0.13710000200099998, 1.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.7105, 0.0080041208559999827, 0.144099972201, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, 1.7942, 0.0080041208559999827, 0.10964997200099999, 1.0, 0.0, 0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 1.7942, 0.0080041208559999827, -0.26590002799900003, 1.0, 0.0, 0.0, -1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.542807172, 0.0080041193959999833, -0.332160027999, 1.0 }; int32_T i; static const int8_T b_iv[27] = { 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, -1, -1, -1, 0, 0, 0, 0, 0, 0, -1, -1, -1, 0, 0, 0 }; static const int8_T b_robot_Home_R[81] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 0, 1, 0, -1, 0, 1, 0, 0, 0, 0, 1, 0, -1, 0, 1, 0, 0, 0, 0, 1, 0, -1, 0, 0, -1, 0, 1, 0, 0, 0, 0, 1, 0, -1, 0, 0, 0, -1, 1, 0, 0, 1, 0, 0, 0, -1, 0, 0, 0, -1, 0, 0, -1, 1, 0, 0, 0, -1, 0, 0, 0, -1, 0, 1, 0, 1, 0, 0 }; int32_T j; int32_T b_i; int32_T T_tmp; int32_T iy; real_T T[16]; int32_T Mlist_CG_Base_tmp; int32_T b_Mlist_CG_Base_tmp; real_T ct_data[27]; real_T Mlist_CG_Base[160]; real_T Mlist_CG[144]; static const int8_T iv1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; real_T dv7[16]; real_T invT_tmp[9]; real_T b_invT_tmp[9]; real_T d; real_T d1; real_T dv8[16]; real_T d2; memcpy(&robot_m[0], &dv[0], 9U * sizeof(real_T)); memcpy(&robot_I[0], &dv1[0], 81U * sizeof(real_T)); memcpy(&robot_com[0], &dv2[0], 27U * sizeof(real_T)); memcpy(&robot_com_pos_R2[0], &dv3[0], 27U * sizeof(real_T)); memcpy(&robot_slist[0], &dv4[0], 54U * sizeof(real_T)); memcpy(&robot_Home_P[0], &dv5[0], 27U * sizeof(real_T)); memcpy(&robot_Home_M[0], &dv6[0], 144U * sizeof(real_T)); robot_gravity[0] = 0.0; robot_gravity[1] = 0.0; robot_gravity[2] = -9.806; *robot_ndof = 9.0; for (i = 0; i < 27; i++) { robot_axis[i] = b_iv[i]; robot_com_pos_R1[i] = robot_com[i]; } memset(&robot_theta[0], 0, 9U * sizeof(real_T)); memset(&robot_dtheta[0], 0, 9U * sizeof(real_T)); memset(&robot_ddtheta[0], 0, 9U * sizeof(real_T)); for (i = 0; i < 81; i++) { robot_Home_R[i] = b_robot_Home_R[i]; } memset(&robot_kine->TW[0], 0, 144U * sizeof(real_T)); memset(&robot_kine->T[0], 0, 144U * sizeof(real_T)); // Get transformation for each joint // TODO:move to lib for (j = 0; j < 9; j++) { for (i = 0; i < 4; i++) { T_tmp = i << 2; iy = T_tmp + (j << 4); T[T_tmp] = robot_Home_M[iy]; T[T_tmp + 1] = robot_Home_M[iy + 1]; T[T_tmp + 2] = robot_Home_M[iy + 2]; T[T_tmp + 3] = robot_Home_M[iy + 3]; } i = static_cast(((-1.0 - (static_cast(j) + 1.0)) + 1.0) / -1.0); emlrtForLoopVectorCheckR2012b(static_cast(j) + 1.0, -1.0, 1.0, mxDOUBLE_CLASS, i, &emlrtRTEI, sp); for (b_i = 0; b_i < i; b_i++) { Mlist_CG_Base_tmp = (j - b_i) + 1; if (Mlist_CG_Base_tmp < 1) { emlrtDynamicBoundsCheckR2012b(Mlist_CG_Base_tmp, 1, 9, &emlrtBCI, sp); } // *** CHAPTER 3: RIGID-BODY MOTIONS *** // Takes a 6-vector (representing a spatial velocity). // Returns the corresponding 4x4 se(3) matrix. // Example Input: // // clear; clc; // V = [1; 2; 3; 4; 5; 6]; // se3mat = VecTose3(V) // // Output: // se3mat = // 0 -3 2 4 // 3 0 -1 5 // -2 1 0 6 // 0 0 0 0 // *** CHAPTER 3: RIGID-BODY MOTIONS *** // Takes a 3-vector (angular velocity). // Returns the skew symmetric matrix in so(3). // Example Input: // // clear; clc; // omg = [1; 2; 3]; // so3mat = VecToso3(omg) // // Output: // so3mat = // 0 -3 2 // 3 0 -1 // -2 1 0 dv7[0] = 0.0; dv7[4] = 0.0; dv7[8] = 0.0; dv7[1] = 0.0; dv7[5] = 0.0; dv7[9] = 0.0; dv7[2] = 0.0; dv7[6] = 0.0; dv7[10] = 0.0; dv7[12] = 0.0; dv7[13] = 0.0; dv7[14] = 0.0; dv7[3] = 0.0; dv7[7] = 0.0; dv7[11] = 0.0; dv7[15] = 0.0; MatrixExp6(dv7, dv8); for (Mlist_CG_Base_tmp = 0; Mlist_CG_Base_tmp < 4; Mlist_CG_Base_tmp++) { d = dv8[Mlist_CG_Base_tmp + 4]; d1 = dv8[Mlist_CG_Base_tmp + 8]; d2 = dv8[Mlist_CG_Base_tmp + 12]; for (b_Mlist_CG_Base_tmp = 0; b_Mlist_CG_Base_tmp < 4; b_Mlist_CG_Base_tmp++) { T_tmp = b_Mlist_CG_Base_tmp << 2; dv7[Mlist_CG_Base_tmp + T_tmp] = ((dv8[Mlist_CG_Base_tmp] * T[T_tmp] + d * T[T_tmp + 1]) + d1 * T[T_tmp + 2]) + d2 * T[T_tmp + 3]; } } memcpy(&T[0], &dv7[0], 16U * sizeof(real_T)); if (*emlrtBreakCheckR2012bFlagVar != 0) { emlrtBreakCheckR2012b(sp); } } for (i = 0; i < 4; i++) { Mlist_CG_Base_tmp = i << 2; b_Mlist_CG_Base_tmp = Mlist_CG_Base_tmp + (j << 4); robot_kine->TW[b_Mlist_CG_Base_tmp] = T[Mlist_CG_Base_tmp]; robot_kine->TW[b_Mlist_CG_Base_tmp + 1] = T[Mlist_CG_Base_tmp + 1]; robot_kine->TW[b_Mlist_CG_Base_tmp + 2] = T[Mlist_CG_Base_tmp + 2]; robot_kine->TW[b_Mlist_CG_Base_tmp + 3] = T[Mlist_CG_Base_tmp + 3]; } if (*emlrtBreakCheckR2012bFlagVar != 0) { emlrtBreakCheckR2012b(sp); } } for (b_i = 0; b_i < 9; b_i++) { if (b_i + 1 == 1) { for (i = 0; i < 4; i++) { Mlist_CG_Base_tmp = i << 2; robot_kine->T[Mlist_CG_Base_tmp] = robot_kine->TW[Mlist_CG_Base_tmp]; b_Mlist_CG_Base_tmp = Mlist_CG_Base_tmp + 1; robot_kine->T[b_Mlist_CG_Base_tmp] = robot_kine->TW[b_Mlist_CG_Base_tmp]; b_Mlist_CG_Base_tmp = Mlist_CG_Base_tmp + 2; robot_kine->T[b_Mlist_CG_Base_tmp] = robot_kine->TW[b_Mlist_CG_Base_tmp]; Mlist_CG_Base_tmp += 3; robot_kine->T[Mlist_CG_Base_tmp] = robot_kine->TW[Mlist_CG_Base_tmp]; } } else { // *** CHAPTER 3: RIGID-BODY MOTIONS *** // Takes a transformation matrix T. // Returns its inverse. // Uses the structure of transformation matrices to avoid taking a matrix // inverse, for efficiency. // Example Input: // // clear; clc; // T = [[1, 0, 0, 0]; [0, 0, -1, 0]; [0, 1, 0, 3]; [0, 0, 0, 1]]; // invT = TransInv(T) // // Ouput: // invT = // 1 0 0 0 // 0 0 1 -3 // 0 -1 0 0 // 0 0 0 1 // *** CHAPTER 3: RIGID-BODY MOTIONS *** // Takes the transformation matrix T in SE(3) // Returns R: the corresponding rotation matrix // p: the corresponding position vector . // Example Input: // // clear; clc; // T = [[1, 0, 0, 0]; [0, 0, -1, 0]; [0, 1, 0, 3]; [0, 0, 0, 1]]; // [R, p] = TransToRp(T) // // Output: // R = // 1 0 0 // 0 0 -1 // 0 1 0 // p = // 0 // 0 // 3 for (i = 0; i < 3; i++) { iy = i + ((b_i - 1) << 4); invT_tmp[3 * i] = robot_kine->TW[iy]; invT_tmp[3 * i + 1] = robot_kine->TW[iy + 4]; invT_tmp[3 * i + 2] = robot_kine->TW[iy + 8]; } for (i = 0; i < 9; i++) { b_invT_tmp[i] = -invT_tmp[i]; } i = (b_i - 1) << 4; for (Mlist_CG_Base_tmp = 0; Mlist_CG_Base_tmp < 3; Mlist_CG_Base_tmp++) { T_tmp = Mlist_CG_Base_tmp << 2; T[T_tmp] = invT_tmp[3 * Mlist_CG_Base_tmp]; T[T_tmp + 1] = invT_tmp[3 * Mlist_CG_Base_tmp + 1]; T[T_tmp + 2] = invT_tmp[3 * Mlist_CG_Base_tmp + 2]; T[Mlist_CG_Base_tmp + 12] = (b_invT_tmp[Mlist_CG_Base_tmp] * robot_kine->TW[i + 12] + b_invT_tmp[Mlist_CG_Base_tmp + 3] * robot_kine->TW[i + 13]) + b_invT_tmp[Mlist_CG_Base_tmp + 6] * robot_kine->TW[i + 14]; } T[3] = 0.0; T[7] = 0.0; T[11] = 0.0; T[15] = 1.0; i = b_i << 4; for (Mlist_CG_Base_tmp = 0; Mlist_CG_Base_tmp < 4; Mlist_CG_Base_tmp++) { d = T[Mlist_CG_Base_tmp + 4]; d1 = T[Mlist_CG_Base_tmp + 8]; d2 = T[Mlist_CG_Base_tmp + 12]; for (b_Mlist_CG_Base_tmp = 0; b_Mlist_CG_Base_tmp < 4; b_Mlist_CG_Base_tmp++) { T_tmp = b_Mlist_CG_Base_tmp << 2; iy = (Mlist_CG_Base_tmp + T_tmp) + i; robot_kine->T[iy] = 0.0; T_tmp += i; robot_kine->T[iy] += T[Mlist_CG_Base_tmp] * robot_kine->TW[T_tmp]; robot_kine->T[iy] += d * robot_kine->TW[T_tmp + 1]; robot_kine->T[iy] += d1 * robot_kine->TW[T_tmp + 2]; robot_kine->T[iy] += d2 * robot_kine->TW[T_tmp + 3]; } } } if (*emlrtBreakCheckR2012bFlagVar != 0) { emlrtBreakCheckR2012b(sp); } } // stack result into kine structure for (i = 0; i < 9; i++) { Mlist_CG_Base_tmp = i << 4; for (b_Mlist_CG_Base_tmp = 0; b_Mlist_CG_Base_tmp < 3; b_Mlist_CG_Base_tmp++) { T_tmp = (b_Mlist_CG_Base_tmp << 2) + Mlist_CG_Base_tmp; iy = 3 * b_Mlist_CG_Base_tmp + 9 * i; robot_kine->R[iy] = robot_kine->T[T_tmp]; robot_kine->R[iy + 1] = robot_kine->T[T_tmp + 1]; robot_kine->R[iy + 2] = robot_kine->T[T_tmp + 2]; robot_kine->t[b_Mlist_CG_Base_tmp + 3 * i] = robot_kine->T [(b_Mlist_CG_Base_tmp + Mlist_CG_Base_tmp) + 12]; } } for (i = 0; i < 4; i++) { Mlist_CG_Base_tmp = i << 2; robot_kine->Fkine[Mlist_CG_Base_tmp] = robot_kine->TW[Mlist_CG_Base_tmp + 128]; robot_kine->Fkine[Mlist_CG_Base_tmp + 1] = robot_kine->TW[Mlist_CG_Base_tmp + 129]; robot_kine->Fkine[Mlist_CG_Base_tmp + 2] = robot_kine->TW[Mlist_CG_Base_tmp + 130]; robot_kine->Fkine[Mlist_CG_Base_tmp + 3] = robot_kine->TW[Mlist_CG_Base_tmp + 131]; } // get the CG at the world base frame // FIXME: Fix this hack memset(&ct_data[0], 0, 27U * sizeof(real_T)); memset(&Mlist_CG_Base[0], 0, 160U * sizeof(real_T)); for (b_i = 0; b_i < 9; b_i++) { if (b_i + 1 == 1) { ct_data[0] = robot_com[0]; ct_data[1] = robot_com[1]; ct_data[2] = robot_com[2]; // elseif i< 9 } else { iy = 3 * (b_i - 1); ct_data[3 * b_i] = (ct_data[iy] - robot_com_pos_R2[iy]) + robot_com[3 * b_i]; T_tmp = 3 * b_i + 1; ct_data[T_tmp] = (ct_data[iy + 1] - robot_com_pos_R2[iy + 1]) + robot_com[T_tmp]; T_tmp = 3 * b_i + 2; ct_data[T_tmp] = (ct_data[iy + 2] - robot_com_pos_R2[iy + 2]) + robot_com[T_tmp]; // else // % HACK // ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0;0;0.05896]+com_pos_R1(:,i); } // *** CHAPTER 3: RIGID-BODY MOTIONS *** // Takes rotation matrix R and position p. // Returns the corresponding homogeneous transformation matrix T in SE(3). // Example Input: // // clear; clc; // R = [[1, 0, 0]; [0, 0, -1]; [0, 1, 0]]; // p = [1; 2; 5]; // T = RpToTrans(R, p) // // Output: // T = // 1 0 0 1 // 0 0 -1 2 // 0 1 0 5 // 0 0 0 1 iy = b_i << 4; for (i = 0; i < 3; i++) { T_tmp = i + 3 * b_i; robot_Home_com[T_tmp] = ct_data[T_tmp]; b_Mlist_CG_Base_tmp = 3 * i + 9 * b_i; Mlist_CG_Base_tmp = (i << 2) + iy; Mlist_CG_Base[Mlist_CG_Base_tmp] = b_robot_Home_R[b_Mlist_CG_Base_tmp]; Mlist_CG_Base[Mlist_CG_Base_tmp + 1] = b_robot_Home_R[b_Mlist_CG_Base_tmp + 1]; Mlist_CG_Base[Mlist_CG_Base_tmp + 2] = b_robot_Home_R[b_Mlist_CG_Base_tmp + 2]; Mlist_CG_Base[(i + iy) + 12] = ct_data[T_tmp]; } b_Mlist_CG_Base_tmp = b_i << 4; Mlist_CG_Base[b_Mlist_CG_Base_tmp + 3] = 0.0; Mlist_CG_Base[b_Mlist_CG_Base_tmp + 7] = 0.0; Mlist_CG_Base[b_Mlist_CG_Base_tmp + 11] = 0.0; Mlist_CG_Base[b_Mlist_CG_Base_tmp + 15] = 1.0; if (*emlrtBreakCheckR2012bFlagVar != 0) { emlrtBreakCheckR2012b(sp); } } memcpy(&robot_kine->Mlist_CG_Base[0], &Mlist_CG_Base[0], 160U * sizeof(real_T)); // get the CG at the last GC frame for (b_i = 0; b_i < 9; b_i++) { if (b_i + 1 == 1) { for (i = 0; i < 4; i++) { iy = i << 2; Mlist_CG[iy] = Mlist_CG_Base[iy]; T_tmp = iy + 1; Mlist_CG[T_tmp] = Mlist_CG_Base[T_tmp]; T_tmp = iy + 2; Mlist_CG[T_tmp] = Mlist_CG_Base[T_tmp]; iy += 3; Mlist_CG[iy] = Mlist_CG_Base[iy]; } } else { // *** CHAPTER 3: RIGID-BODY MOTIONS *** // Takes a transformation matrix T. // Returns its inverse. // Uses the structure of transformation matrices to avoid taking a matrix // inverse, for efficiency. // Example Input: // // clear; clc; // T = [[1, 0, 0, 0]; [0, 0, -1, 0]; [0, 1, 0, 3]; [0, 0, 0, 1]]; // invT = TransInv(T) // // Ouput: // invT = // 1 0 0 0 // 0 0 1 -3 // 0 -1 0 0 // 0 0 0 1 // *** CHAPTER 3: RIGID-BODY MOTIONS *** // Takes the transformation matrix T in SE(3) // Returns R: the corresponding rotation matrix // p: the corresponding position vector . // Example Input: // // clear; clc; // T = [[1, 0, 0, 0]; [0, 0, -1, 0]; [0, 1, 0, 3]; [0, 0, 0, 1]]; // [R, p] = TransToRp(T) // // Output: // R = // 1 0 0 // 0 0 -1 // 0 1 0 // p = // 0 // 0 // 3 for (i = 0; i < 3; i++) { iy = i + ((b_i - 1) << 4); invT_tmp[3 * i] = Mlist_CG_Base[iy]; invT_tmp[3 * i + 1] = Mlist_CG_Base[iy + 4]; invT_tmp[3 * i + 2] = Mlist_CG_Base[iy + 8]; } for (i = 0; i < 9; i++) { b_invT_tmp[i] = -invT_tmp[i]; } i = (b_i - 1) << 4; for (Mlist_CG_Base_tmp = 0; Mlist_CG_Base_tmp < 3; Mlist_CG_Base_tmp++) { T_tmp = Mlist_CG_Base_tmp << 2; T[T_tmp] = invT_tmp[3 * Mlist_CG_Base_tmp]; T[T_tmp + 1] = invT_tmp[3 * Mlist_CG_Base_tmp + 1]; T[T_tmp + 2] = invT_tmp[3 * Mlist_CG_Base_tmp + 2]; T[Mlist_CG_Base_tmp + 12] = (b_invT_tmp[Mlist_CG_Base_tmp] * Mlist_CG_Base[i + 12] + b_invT_tmp[Mlist_CG_Base_tmp + 3] * Mlist_CG_Base[i + 13]) + b_invT_tmp[Mlist_CG_Base_tmp + 6] * Mlist_CG_Base[i + 14]; } T[3] = 0.0; T[7] = 0.0; T[11] = 0.0; T[15] = 1.0; i = b_i << 4; for (Mlist_CG_Base_tmp = 0; Mlist_CG_Base_tmp < 4; Mlist_CG_Base_tmp++) { d = T[Mlist_CG_Base_tmp + 4]; d1 = T[Mlist_CG_Base_tmp + 8]; d2 = T[Mlist_CG_Base_tmp + 12]; for (b_Mlist_CG_Base_tmp = 0; b_Mlist_CG_Base_tmp < 4; b_Mlist_CG_Base_tmp++) { T_tmp = b_Mlist_CG_Base_tmp << 2; iy = T_tmp + i; Mlist_CG[(Mlist_CG_Base_tmp + T_tmp) + i] = ((T[Mlist_CG_Base_tmp] * Mlist_CG_Base[iy] + d * Mlist_CG_Base[iy + 1]) + d1 * Mlist_CG_Base[iy + 2]) + d2 * Mlist_CG_Base[iy + 3]; } } } if (*emlrtBreakCheckR2012bFlagVar != 0) { emlrtBreakCheckR2012b(sp); } } iy = -1; for (j = 0; j < 144; j++) { iy++; Mlist_CG_Base[iy] = Mlist_CG[j]; } for (j = 0; j < 16; j++) { iy++; Mlist_CG_Base[iy] = iv1[j]; } memcpy(&robot_kine->Mlist_CG[0], &Mlist_CG_Base[0], 160U * sizeof(real_T)); // Get the end efforce transformation in each joint reference frame for (b_i = 0; b_i < 9; b_i++) { for (i = 0; i < 4; i++) { iy = (i << 2) + (b_i << 4); Mlist_CG[iy] = robot_kine->T[iy]; T_tmp = iy + 1; Mlist_CG[T_tmp] = robot_kine->T[T_tmp]; T_tmp = iy + 2; Mlist_CG[T_tmp] = robot_kine->T[T_tmp]; iy += 3; Mlist_CG[iy] = robot_kine->T[iy]; } if (*emlrtBreakCheckR2012bFlagVar != 0) { emlrtBreakCheckR2012b(sp); } } iy = -1; for (j = 0; j < 144; j++) { iy++; Mlist_CG_Base[iy] = Mlist_CG[j]; } for (j = 0; j < 16; j++) { iy++; Mlist_CG_Base[iy] = iv1[j]; } memcpy(&robot_kine->Mlist_ED[0], &Mlist_CG_Base[0], 160U * sizeof(real_T)); } // End of code generation (get_Kinematics.cpp)