149 lines
5.0 KiB
C++
149 lines
5.0 KiB
C++
//
|
|
// 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.
|
|
//
|
|
// getGravityForce.cpp
|
|
//
|
|
// Code generation for function 'getGravityForce'
|
|
//
|
|
|
|
|
|
// Include files
|
|
#include "getGravityForce.h"
|
|
#include "FKinSpaceExpand.h"
|
|
#include "InverseDynamics_debug.h"
|
|
#include "calculateGravityForce.h"
|
|
#include "calculateGravityForce_data.h"
|
|
#include "getSimpackF.h"
|
|
#include "rt_nonfinite.h"
|
|
|
|
// Variable Definitions
|
|
static emlrtRSInfo c_emlrtRSI = { 23, // lineNo
|
|
"getGravityForce", // fcnName
|
|
"C:\\R1000 EVT GravityForce V1\\getGravityForce.m"// pathName
|
|
};
|
|
|
|
static emlrtRSInfo d_emlrtRSI = { 24, // lineNo
|
|
"getGravityForce", // fcnName
|
|
"C:\\R1000 EVT GravityForce V1\\getGravityForce.m"// pathName
|
|
};
|
|
|
|
static emlrtRSInfo f_emlrtRSI = { 20, // lineNo
|
|
"getGravityForce", // fcnName
|
|
"C:\\R1000 EVT GravityForce V1\\getGravityForce.m"// pathName
|
|
};
|
|
|
|
static emlrtRSInfo g_emlrtRSI = { 25, // lineNo
|
|
"getGravityForce", // fcnName
|
|
"C:\\R1000 EVT GravityForce V1\\getGravityForce.m"// pathName
|
|
};
|
|
|
|
// Function Definitions
|
|
void getGravityForce(const emlrtStack *sp, const real_T thetalist[9], const
|
|
real_T robot_m[9], const real_T robot_I[81], const real_T
|
|
robot_slist[54], const real_T robot_Home_R[81], const
|
|
struct_T *robot_kine, const real_T robot_gravity[3], real_T
|
|
GravityForce[6])
|
|
{
|
|
int32_T b_i;
|
|
real_T dv[6];
|
|
real_T Glist_data[324];
|
|
real_T unusedU0[60];
|
|
real_T unusedU1[60];
|
|
real_T unusedU2[360];
|
|
real_T Fmat[54];
|
|
real_T b_robot_Home_R[9];
|
|
real_T d;
|
|
int32_T Glist_data_tmp;
|
|
real_T G[144];
|
|
real_T T[144];
|
|
real_T F_Simpack[54];
|
|
real_T link_inertia_data[81];
|
|
static const int8_T a[36] = { 0, -1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, -1, 0,
|
|
0, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, -1 };
|
|
|
|
emlrtStack st;
|
|
st.prev = sp;
|
|
st.tls = sp->tls;
|
|
|
|
// Get info from robot
|
|
// Get general mass matrix
|
|
for (int32_T i = 0; i < 9; i++) {
|
|
int32_T b_Glist_data_tmp;
|
|
int32_T c_Glist_data_tmp;
|
|
for (b_i = 0; b_i < 3; b_i++) {
|
|
real_T d1;
|
|
Glist_data_tmp = 3 * b_i + 9 * i;
|
|
for (b_Glist_data_tmp = 0; b_Glist_data_tmp < 3; b_Glist_data_tmp++) {
|
|
c_Glist_data_tmp = 3 * b_Glist_data_tmp + 9 * i;
|
|
b_robot_Home_R[b_i + 3 * b_Glist_data_tmp] =
|
|
(robot_Home_R[Glist_data_tmp] * robot_I[c_Glist_data_tmp] +
|
|
robot_Home_R[Glist_data_tmp + 1] * robot_I[c_Glist_data_tmp + 1]) +
|
|
robot_Home_R[Glist_data_tmp + 2] * robot_I[c_Glist_data_tmp + 2];
|
|
}
|
|
|
|
d = b_robot_Home_R[b_i + 3];
|
|
d1 = b_robot_Home_R[b_i + 6];
|
|
for (Glist_data_tmp = 0; Glist_data_tmp < 3; Glist_data_tmp++) {
|
|
b_Glist_data_tmp = 3 * Glist_data_tmp + 9 * i;
|
|
link_inertia_data[(b_i + 3 * Glist_data_tmp) + 9 * i] =
|
|
(b_robot_Home_R[b_i] * robot_Home_R[b_Glist_data_tmp] + d *
|
|
robot_Home_R[b_Glist_data_tmp + 1]) + d1 *
|
|
robot_Home_R[b_Glist_data_tmp + 2];
|
|
}
|
|
}
|
|
|
|
d = robot_m[i];
|
|
for (b_i = 0; b_i < 3; b_i++) {
|
|
c_Glist_data_tmp = 6 * b_i + 36 * i;
|
|
Glist_data_tmp = 3 * b_i + 9 * i;
|
|
Glist_data[c_Glist_data_tmp] = link_inertia_data[Glist_data_tmp];
|
|
b_Glist_data_tmp = 6 * (b_i + 3) + 36 * i;
|
|
Glist_data[b_Glist_data_tmp] = 0.0;
|
|
Glist_data[c_Glist_data_tmp + 3] = 0.0;
|
|
Glist_data[b_Glist_data_tmp + 3] = d * static_cast<real_T>(iv[3 * b_i]);
|
|
Glist_data[c_Glist_data_tmp + 1] = link_inertia_data[Glist_data_tmp + 1];
|
|
Glist_data[b_Glist_data_tmp + 1] = 0.0;
|
|
Glist_data[c_Glist_data_tmp + 4] = 0.0;
|
|
Glist_data[b_Glist_data_tmp + 4] = d * static_cast<real_T>(iv[3 * b_i + 1]);
|
|
Glist_data[c_Glist_data_tmp + 2] = link_inertia_data[Glist_data_tmp + 2];
|
|
Glist_data[b_Glist_data_tmp + 2] = 0.0;
|
|
Glist_data[c_Glist_data_tmp + 5] = 0.0;
|
|
Glist_data[b_Glist_data_tmp + 5] = d * static_cast<real_T>(iv[3 * b_i + 2]);
|
|
}
|
|
|
|
if (*emlrtBreakCheckR2012bFlagVar != 0) {
|
|
emlrtBreakCheckR2012b(sp);
|
|
}
|
|
}
|
|
|
|
for (b_i = 0; b_i < 6; b_i++) {
|
|
dv[b_i] = 0.0;
|
|
}
|
|
|
|
st.site = &f_emlrtRSI;
|
|
InverseDynamics_debug(&st, thetalist, robot_gravity, dv, robot_kine->Mlist_CG,
|
|
Glist_data, robot_slist, unusedU0, unusedU1, unusedU2,
|
|
Fmat, b_robot_Home_R);
|
|
st.site = &c_emlrtRSI;
|
|
FKinSpaceExpand(&st, robot_kine->Mlist_CG, robot_slist, thetalist, G);
|
|
st.site = &d_emlrtRSI;
|
|
FKinSpaceExpand(&st, robot_kine->Mlist_ED, robot_slist, thetalist, T);
|
|
st.site = &g_emlrtRSI;
|
|
getSimpackF(&st, G, T, Fmat, F_Simpack);
|
|
|
|
// get J7
|
|
for (b_i = 0; b_i < 6; b_i++) {
|
|
d = 0.0;
|
|
for (Glist_data_tmp = 0; Glist_data_tmp < 6; Glist_data_tmp++) {
|
|
d += static_cast<real_T>(a[b_i + 6 * Glist_data_tmp]) *
|
|
F_Simpack[Glist_data_tmp + 36];
|
|
}
|
|
|
|
GravityForce[b_i] = d;
|
|
}
|
|
}
|
|
|
|
// End of code generation (getGravityForce.cpp)
|