Robotics
/
IRDYn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
IRDYn/complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityForce/get_Kinematics.cpp

612 lines
22 KiB
C++
Raw Blame History

//
// 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 <string.h>
// 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<int32_T>(((-1.0 - (static_cast<real_T>(j) + 1.0)) + 1.0) /
-1.0);
emlrtForLoopVectorCheckR2012b(static_cast<real_T>(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)
Reference in New Issue View Git Blame Copy Permalink