218 lines
6.3 KiB
C++
218 lines
6.3 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.
|
|
//
|
|
// getSimpackF.cpp
|
|
//
|
|
// Code generation for function 'getSimpackF'
|
|
//
|
|
|
|
|
|
// Include files
|
|
#include "getSimpackF.h"
|
|
#include "calculateGravityForce.h"
|
|
#include "calculateGravityForce_data.h"
|
|
#include "rt_nonfinite.h"
|
|
|
|
// Function Definitions
|
|
void getSimpackF(const emlrtStack *sp, const real_T T_GC[144], const real_T T_O
|
|
[144], const real_T Fmat[54], real_T Flist[54])
|
|
{
|
|
real_T invT_tmp[9];
|
|
real_T b_invT_tmp[9];
|
|
real_T c_invT_tmp[16];
|
|
real_T s;
|
|
real_T y[16];
|
|
real_T Adgab[36];
|
|
|
|
// base frame
|
|
// Adgab = Adjoint(TransInv(T_GC(:,:,1))*Mlist(:,:,1));
|
|
// Flist(:,1) = Adgab'*Fmat(1,:)';
|
|
// for i = 2: N
|
|
// % Adgab = Adjoint(TransInv(T_GC(:,:,i))*T_O(:,:,i-1)*Mlist(:,:,i));
|
|
// Adgab = Adjoint(TransInv(T_GC(:,:,i))*T_O(:,:,i));
|
|
// Flist(:,i) = Adgab'*Fmat(i,:)';
|
|
// endAdgab = Adjoint(TransInv(T_GC(:,:,1))*Mlist(:,:,1));
|
|
for (int32_T i = 0; i < 9; i++) {
|
|
int32_T b_i;
|
|
int32_T aoffset;
|
|
int32_T k;
|
|
real_T d;
|
|
int32_T Adgab_tmp;
|
|
|
|
// Adgab = Adjoint(TransInv(T_GC(:,:,i))*T_O(:,:,i-1)*Mlist(:,:,i));
|
|
// *** 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 (b_i = 0; b_i < 3; b_i++) {
|
|
aoffset = b_i + (i << 4);
|
|
invT_tmp[3 * b_i] = T_GC[aoffset];
|
|
invT_tmp[3 * b_i + 1] = T_GC[aoffset + 4];
|
|
invT_tmp[3 * b_i + 2] = T_GC[aoffset + 8];
|
|
}
|
|
|
|
for (b_i = 0; b_i < 9; b_i++) {
|
|
b_invT_tmp[b_i] = -invT_tmp[b_i];
|
|
}
|
|
|
|
b_i = i << 4;
|
|
for (k = 0; k < 3; k++) {
|
|
aoffset = k << 2;
|
|
c_invT_tmp[aoffset] = invT_tmp[3 * k];
|
|
c_invT_tmp[aoffset + 1] = invT_tmp[3 * k + 1];
|
|
c_invT_tmp[aoffset + 2] = invT_tmp[3 * k + 2];
|
|
c_invT_tmp[k + 12] = (b_invT_tmp[k] * T_GC[b_i + 12] + b_invT_tmp[k + 3] *
|
|
T_GC[b_i + 13]) + b_invT_tmp[k + 6] * T_GC[b_i + 14];
|
|
}
|
|
|
|
c_invT_tmp[3] = 0.0;
|
|
c_invT_tmp[7] = 0.0;
|
|
c_invT_tmp[11] = 0.0;
|
|
c_invT_tmp[15] = 1.0;
|
|
for (b_i = 0; b_i < 4; b_i++) {
|
|
real_T d1;
|
|
s = c_invT_tmp[b_i + 4];
|
|
d = c_invT_tmp[b_i + 8];
|
|
d1 = c_invT_tmp[b_i + 12];
|
|
for (k = 0; k < 4; k++) {
|
|
Adgab_tmp = k << 2;
|
|
aoffset = Adgab_tmp + (i << 4);
|
|
y[b_i + Adgab_tmp] = ((c_invT_tmp[b_i] * T_O[aoffset] + s * T_O[aoffset
|
|
+ 1]) + d * T_O[aoffset + 2]) + d1 * T_O[aoffset + 3];
|
|
}
|
|
}
|
|
|
|
// *** CHAPTER 3: RIGID-BODY MOTIONS ***
|
|
// Takes T a transformation matrix SE3.
|
|
// Returns the corresponding 6x6 adjoint representation [AdT].
|
|
// Example Input:
|
|
//
|
|
// clear; clc;
|
|
// T = [[1, 0, 0, 0]; [0, 0, -1, 0]; [0, 1, 0, 3]; [0, 0, 0, 1]];
|
|
// AdT = Adjoint(T)
|
|
//
|
|
// Output:
|
|
// AdT =
|
|
// 1 0 0 0 0 0
|
|
// 0 0 -1 0 0 0
|
|
// 0 1 0 0 0 0
|
|
// 0 0 3 1 0 0
|
|
// 3 0 0 0 0 -1
|
|
// 0 0 0 0 1 0
|
|
// *** 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
|
|
// *** 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
|
|
invT_tmp[0] = 0.0;
|
|
invT_tmp[3] = -y[14];
|
|
invT_tmp[6] = y[13];
|
|
invT_tmp[1] = y[14];
|
|
invT_tmp[4] = 0.0;
|
|
invT_tmp[7] = -y[12];
|
|
invT_tmp[2] = -y[13];
|
|
invT_tmp[5] = y[12];
|
|
invT_tmp[8] = 0.0;
|
|
for (b_i = 0; b_i < 3; b_i++) {
|
|
s = invT_tmp[b_i + 3];
|
|
d = invT_tmp[b_i + 6];
|
|
for (k = 0; k < 3; k++) {
|
|
Adgab_tmp = k << 2;
|
|
b_invT_tmp[b_i + 3 * k] = (invT_tmp[b_i] * y[Adgab_tmp] + s *
|
|
y[Adgab_tmp + 1]) + d * y[Adgab_tmp + 2];
|
|
Adgab[k + 6 * b_i] = y[k + (b_i << 2)];
|
|
Adgab[k + 6 * (b_i + 3)] = 0.0;
|
|
}
|
|
}
|
|
|
|
for (b_i = 0; b_i < 3; b_i++) {
|
|
Adgab[6 * b_i + 3] = b_invT_tmp[3 * b_i];
|
|
aoffset = b_i << 2;
|
|
Adgab_tmp = 6 * (b_i + 3);
|
|
Adgab[Adgab_tmp + 3] = y[aoffset];
|
|
Adgab[6 * b_i + 4] = b_invT_tmp[3 * b_i + 1];
|
|
Adgab[Adgab_tmp + 4] = y[aoffset + 1];
|
|
Adgab[6 * b_i + 5] = b_invT_tmp[3 * b_i + 2];
|
|
Adgab[Adgab_tmp + 5] = y[aoffset + 2];
|
|
}
|
|
|
|
for (Adgab_tmp = 0; Adgab_tmp < 6; Adgab_tmp++) {
|
|
aoffset = Adgab_tmp * 6;
|
|
s = 0.0;
|
|
for (k = 0; k < 6; k++) {
|
|
s += Adgab[aoffset + k] * Fmat[i + 9 * k];
|
|
}
|
|
|
|
Flist[Adgab_tmp + 6 * i] = s;
|
|
}
|
|
|
|
if (*emlrtBreakCheckR2012bFlagVar != 0) {
|
|
emlrtBreakCheckR2012b(sp);
|
|
}
|
|
}
|
|
}
|
|
|
|
// End of code generation (getSimpackF.cpp)
|