274 lines
8.0 KiB
C++
274 lines
8.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.
|
||
|
|
//
|
||
|
|
// 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 <string.h>
|
||
|
|
|
||
|
|
// 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<int32_T>(((-1.0 - static_cast<real_T>(i)) + 1.0) / -1.0);
|
||
|
|
emlrtForLoopVectorCheckR2012b(9.0 - static_cast<real_T>(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)
|