// // 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. // // FKinSpaceExpand.cpp // // Code generation for function 'FKinSpaceExpand' // // Include files #include "FKinSpaceExpand.h" #include "MatrixExp6.h" #include "calculateGravityForce.h" #include "calculateGravityForce_data.h" #include "rt_nonfinite.h" #include // Variable Definitions static emlrtRSInfo e_emlrtRSI = { 33, // lineNo "FKinSpaceExpand", // fcnName "C:\\R1000 EVT GravityForce V1\\mr\\FKinSpaceExpand.m"// pathName }; static emlrtRTEInfo b_emlrtRTEI = { 28,// lineNo 9, // colNo "FKinSpace", // fName "C:\\R1000 EVT GravityForce V1\\mr\\FKinSpace.m"// pName }; static emlrtBCInfo b_emlrtBCI = { -1, // iFirst -1, // iLast 29, // lineNo 38, // colNo "Slist", // aName "FKinSpace", // fName "C:\\R1000 EVT GravityForce V1\\mr\\FKinSpace.m",// pName 0 // checkKind }; static emlrtBCInfo c_emlrtBCI = { -1, // iFirst -1, // iLast 29, // lineNo 53, // colNo "thetalist", // aName "FKinSpace", // fName "C:\\R1000 EVT GravityForce V1\\mr\\FKinSpace.m",// pName 0 // checkKind }; // Function Definitions void FKinSpaceExpand(const emlrtStack *sp, const real_T Mlist[160], const real_T Slist[54], const real_T thetalist[9], real_T Tlist[144]) { int32_T i; int8_T Mi_tmp[16]; real_T Mi[16]; real_T b_Mi[16]; int32_T i4; int32_T i5; real_T y[6]; emlrtStack st; st.prev = sp; st.tls = sp->tls; // *** CHAPTER 4: FORWARD KINEMATICS *** // Takes M: the home configuration (position and orientation) of the // end-effector, // Slist: The joint screw axes in the space frame when the manipulator // is at the home position, // thetalist: A list of joint coordinates. // Returns T in SE(3) representing the end-effector frame, when the joints // are at the specified coordinates (i.t.o Space Frame). // Example Inputs: // // clear; clc; // M = [[-1, 0, 0, 0]; [0, 1, 0, 6]; [0, 0, -1, 2]; [0, 0, 0, 1]]; // Slist = [[0; 0; 1; 4; 0; 0], ... // [0; 0; 0; 0; 1; 0], ... // [0; 0; -1; -6; 0; -0.1]]; // thetalist =[pi / 2; 3; pi]; // T = FKinSpace(M, Slist, thetalist) // // Output: // T = // -0.0000 1.0000 0 -5.0000 // 1.0000 0.0000 0 4.0000 // 0 0 -1.0000 1.6858 // 0 0 0 1.0000 for (i = 0; i < 16; i++) { Mi_tmp[i] = 0; } Mi_tmp[0] = 1; Mi_tmp[5] = 1; Mi_tmp[10] = 1; Mi_tmp[15] = 1; for (i = 0; i < 16; i++) { Mi[i] = Mi_tmp[i]; } for (int32_T b_i = 0; b_i < 9; b_i++) { int32_T j; int32_T i1; real_T d; real_T d1; real_T d2; int32_T i2; int32_T i3; int32_T Tlist_tmp; i = 8 - b_i; for (j = 0; j <= i; j++) { for (i1 = 0; i1 < 4; i1++) { d = Mi[i1 + 4]; d1 = Mi[i1 + 8]; d2 = Mi[i1 + 12]; for (i2 = 0; i2 < 4; i2++) { i3 = i2 << 2; Tlist_tmp = i3 + (j << 4); b_Mi[i1 + i3] = ((Mi[i1] * Mlist[Tlist_tmp] + d * Mlist[Tlist_tmp + 1]) + d1 * Mlist[Tlist_tmp + 2]) + d2 * Mlist[Tlist_tmp + 3]; } } memcpy(&Mi[0], &b_Mi[0], 16U * sizeof(real_T)); if (*emlrtBreakCheckR2012bFlagVar != 0) { emlrtBreakCheckR2012b(sp); } } st.site = &e_emlrtRSI; // *** CHAPTER 4: FORWARD KINEMATICS *** // Takes M: the home configuration (position and orientation) of the // end-effector, // Slist: The joint screw axes in the space frame when the manipulator // is at the home position, // thetalist: A list of joint coordinates. // Returns T in SE(3) representing the end-effector frame, when the joints // are at the specified coordinates (i.t.o Space Frame). // Example Inputs: // // clear; clc; // M = [[-1, 0, 0, 0]; [0, 1, 0, 6]; [0, 0, -1, 2]; [0, 0, 0, 1]]; // Slist = [[0; 0; 1; 4; 0; 0], ... // [0; 0; 0; 0; 1; 0], ... // [0; 0; -1; -6; 0; -0.1]]; // thetalist =[pi / 2; 3; pi]; // T = FKinSpace(M, Slist, thetalist) // // Output: // T = // -0.0000 1.0000 0 -5.0000 // 1.0000 0.0000 0 4.0000 // 0 0 -1.0000 1.6858 // 0 0 0 1.0000 for (i = 0; i < 4; i++) { j = i << 2; Tlist_tmp = j + ((8 - b_i) << 4); Tlist[Tlist_tmp] = Mi[j]; Tlist[Tlist_tmp + 1] = Mi[j + 1]; Tlist[Tlist_tmp + 2] = Mi[j + 2]; Tlist[Tlist_tmp + 3] = Mi[j + 3]; } i = 9 - b_i; i1 = static_cast(((-1.0 - static_cast(i)) + 1.0) / -1.0); emlrtForLoopVectorCheckR2012b(9.0 - static_cast(b_i), -1.0, 1.0, mxDOUBLE_CLASS, i1, &b_emlrtRTEI, &st); if (0 <= i1 - 1) { i4 = 9 - b_i; i5 = 9 - b_i; } for (int32_T c_i = 0; c_i < i1; c_i++) { j = i - c_i; if ((j < 1) || (j > i4)) { emlrtDynamicBoundsCheckR2012b(j, 1, i4, &b_emlrtBCI, &st); } if (j > i5) { emlrtDynamicBoundsCheckR2012b(j, 1, i5, &c_emlrtBCI, &st); } for (i2 = 0; i2 < 6; i2++) { y[i2] = Slist[i2 + 6 * (j - 1)] * thetalist[j - 1]; } // *** 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 b_Mi[0] = 0.0; b_Mi[4] = -y[2]; b_Mi[8] = y[1]; b_Mi[1] = y[2]; b_Mi[5] = 0.0; b_Mi[9] = -y[0]; b_Mi[2] = -y[1]; b_Mi[6] = y[0]; b_Mi[10] = 0.0; b_Mi[12] = y[3]; b_Mi[13] = y[4]; b_Mi[14] = y[5]; b_Mi[3] = 0.0; b_Mi[7] = 0.0; b_Mi[11] = 0.0; b_Mi[15] = 0.0; MatrixExp6(b_Mi, Mi); for (i2 = 0; i2 < 4; i2++) { d = Mi[i2 + 4]; d1 = Mi[i2 + 8]; d2 = Mi[i2 + 12]; for (i3 = 0; i3 < 4; i3++) { Tlist_tmp = i3 << 2; j = Tlist_tmp + ((8 - b_i) << 4); b_Mi[i2 + Tlist_tmp] = ((Mi[i2] * Tlist[j] + d * Tlist[j + 1]) + d1 * Tlist[j + 2]) + d2 * Tlist[j + 3]; } } for (i2 = 0; i2 < 4; i2++) { j = i2 << 2; Tlist_tmp = j + ((8 - b_i) << 4); Tlist[Tlist_tmp] = b_Mi[j]; Tlist[Tlist_tmp + 1] = b_Mi[j + 1]; Tlist[Tlist_tmp + 2] = b_Mi[j + 2]; Tlist[Tlist_tmp + 3] = b_Mi[j + 3]; } if (*emlrtBreakCheckR2012bFlagVar != 0) { emlrtBreakCheckR2012b(&st); } } for (i = 0; i < 16; i++) { Mi[i] = Mi_tmp[i]; } if (*emlrtBreakCheckR2012bFlagVar != 0) { emlrtBreakCheckR2012b(sp); } } } // End of code generation (FKinSpaceExpand.cpp)