Robotics
/
IRDYn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
IRDYn/complie/R1000 EVT GravityForce V1/codegen/mex/calculateGravityForce/InverseDynamics_debug.cpp

874 lines
24 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.
//
// InverseDynamics_debug.cpp
//
// Code generation for function 'InverseDynamics_debug'
//
// Include files
#include "InverseDynamics_debug.h"
#include "MatrixExp6.h"
#include "calculateGravityForce.h"
#include "calculateGravityForce_data.h"
#include "rt_nonfinite.h"
#include <string.h>
// Function Definitions
void InverseDynamics_debug(const emlrtStack *sp, const real_T thetalist[9],
const real_T g[3], const real_T Ftip[6], const real_T Mlist[160], const real_T
Glist_data[], const real_T Slist[54], real_T Vi[60], real_T Vdi[60], real_T
AdTi[360], real_T Flist[54], real_T taulist[9])
{
real_T Mi[16];
int32_T i;
real_T omgmat[9];
real_T b_omgmat[9];
real_T invT[16];
int32_T invT_tmp;
real_T d;
real_T Fi;
int32_T b_i;
int32_T i1;
int32_T AdTi_tmp;
real_T b_Fi[6];
int32_T b_AdTi_tmp;
real_T y[16];
real_T b_invT[36];
real_T b_y[6];
real_T b_AdTi[6];
real_T c_omgmat[6];
real_T Ai[54];
real_T d_omgmat[16];
// *** CHAPTER 8: DYNAMICS OF OPEN CHAINS ***
// Takes thetalist: n-vector of joint variables,
// dthetalist: n-vector of joint rates,
// ddthetalist: n-vector of joint accelerations,
// g: Gravity vector g,
// Ftip: Spatial force applied by the end-effector expressed in frame
// {n+1},
// Mlist: List of link frames {i} relative to {i-1} at the home
// position,
// Glist: Spatial inertia matrices Gi of the links,
// Slist: Screw axes Si of the joints in a space frame, in the format
// of a matrix with the screw axes as the columns.
// Returns taulist: The n-vector of required joint forces/torques.
// This function uses forward-backward Newton-Euler iterations to solve the
// equation:
// taulist = Mlist(thetalist) * ddthetalist + c(thetalist, dthetalist) ...
// + g(thetalist) + Jtr(thetalist) * Ftip
// Example Input (3 Link Robot):
//
// clear; clc;
// thetalist = [0.1; 0.1; 0.1];
// dthetalist = [0.1; 0.2; 0.3];
// ddthetalist = [2; 1.5; 1];
// g = [0; 0; -9.8];
// Ftip = [1; 1; 1; 1; 1; 1];
// M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.089159]; [0, 0, 0, 1]];
// M12 = [[0, 0, 1, 0.28]; [0, 1, 0, 0.13585]; [-1, 0 ,0, 0]; [0, 0, 0, 1]];
// M23 = [[1, 0, 0, 0]; [0, 1, 0, -0.1197]; [0, 0, 1, 0.395]; [0, 0, 0, 1]];
// M34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.14225]; [0, 0, 0, 1]];
// G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]);
// G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]);
// G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]);
// Glist = cat(3, G1, G2, G3);
// Mlist = cat(3, M01, M12, M23, M34);
// Slist = [[1; 0; 1; 0; 1; 0], ...
// [0; 1; 0; -0.089; 0; 0], ...
// [0; 1; 0; -0.089; 0; 0.425]];
// taulist = InverseDynamics(thetalist, dthetalist, ddthetalist, g, ...
// Ftip, Mlist, Glist, Slist)
//
// Output:
// taulist =
// 74.6962
// -33.0677
// -3.2306
memset(&Mi[0], 0, 16U * sizeof(real_T));
Mi[0] = 1.0;
Mi[5] = 1.0;
Mi[10] = 1.0;
Mi[15] = 1.0;
memset(&AdTi[0], 0, 360U * sizeof(real_T));
memset(&Vi[0], 0, 60U * sizeof(real_T));
memset(&Vdi[0], 0, 60U * sizeof(real_T));
// *** 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++) {
Vdi[i + 3] = -g[i];
omgmat[3 * i] = Mlist[i + 144];
omgmat[3 * i + 1] = Mlist[i + 148];
omgmat[3 * i + 2] = Mlist[i + 152];
}
for (i = 0; i < 9; i++) {
b_omgmat[i] = -omgmat[i];
}
for (i = 0; i < 3; i++) {
invT_tmp = i << 2;
invT[invT_tmp] = omgmat[3 * i];
invT[invT_tmp + 1] = omgmat[3 * i + 1];
invT[invT_tmp + 2] = omgmat[3 * i + 2];
invT[i + 12] = (b_omgmat[i] * Mlist[156] + b_omgmat[i + 3] * Mlist[157]) +
b_omgmat[i + 6] * Mlist[158];
}
invT[3] = 0.0;
invT[7] = 0.0;
invT[11] = 0.0;
invT[15] = 1.0;
// *** 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
omgmat[0] = 0.0;
omgmat[3] = -invT[14];
omgmat[6] = invT[13];
omgmat[1] = invT[14];
omgmat[4] = 0.0;
omgmat[7] = -invT[12];
omgmat[2] = -invT[13];
omgmat[5] = invT[12];
omgmat[8] = 0.0;
for (i = 0; i < 3; i++) {
d = omgmat[i + 3];
Fi = omgmat[i + 6];
for (i1 = 0; i1 < 3; i1++) {
b_AdTi_tmp = i1 << 2;
b_omgmat[i + 3 * i1] = (omgmat[i] * invT[b_AdTi_tmp] + d * invT[b_AdTi_tmp
+ 1]) + Fi * invT[b_AdTi_tmp + 2];
AdTi[(i1 + 6 * i) + 324] = invT[i1 + (i << 2)];
AdTi[(i1 + 6 * (i + 3)) + 324] = 0.0;
}
}
for (i = 0; i < 3; i++) {
AdTi[6 * i + 327] = b_omgmat[3 * i];
AdTi_tmp = i << 2;
invT_tmp = 6 * (i + 3);
AdTi[invT_tmp + 327] = invT[AdTi_tmp];
AdTi[6 * i + 328] = b_omgmat[3 * i + 1];
AdTi[invT_tmp + 328] = invT[AdTi_tmp + 1];
AdTi[6 * i + 329] = b_omgmat[3 * i + 2];
AdTi[invT_tmp + 329] = invT[AdTi_tmp + 2];
}
for (b_i = 0; b_i < 6; b_i++) {
b_Fi[b_i] = Ftip[b_i];
}
for (b_i = 0; b_i < 9; b_i++) {
real_T d1;
for (i = 0; i < 4; i++) {
d = Mi[i + 4];
Fi = Mi[i + 8];
d1 = Mi[i + 12];
for (i1 = 0; i1 < 4; i1++) {
b_AdTi_tmp = i1 << 2;
invT_tmp = b_AdTi_tmp + (b_i << 4);
y[i + b_AdTi_tmp] = ((Mi[i] * Mlist[invT_tmp] + d * Mlist[invT_tmp + 1])
+ Fi * Mlist[invT_tmp + 2]) + d1 * Mlist[invT_tmp +
3];
}
}
memcpy(&Mi[0], &y[0], 16U * sizeof(real_T));
// *** 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++) {
omgmat[3 * i] = Mi[i];
omgmat[3 * i + 1] = Mi[i + 4];
omgmat[3 * i + 2] = Mi[i + 8];
}
for (i = 0; i < 9; i++) {
b_omgmat[i] = -omgmat[i];
}
for (i = 0; i < 3; i++) {
invT_tmp = i << 2;
invT[invT_tmp] = omgmat[3 * i];
invT[invT_tmp + 1] = omgmat[3 * i + 1];
invT[invT_tmp + 2] = omgmat[3 * i + 2];
invT[i + 12] = (b_omgmat[i] * Mi[12] + b_omgmat[i + 3] * Mi[13]) +
b_omgmat[i + 6] * Mi[14];
}
invT[3] = 0.0;
invT[7] = 0.0;
invT[11] = 0.0;
invT[15] = 1.0;
// *** 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
omgmat[0] = 0.0;
omgmat[3] = -invT[14];
omgmat[6] = invT[13];
omgmat[1] = invT[14];
omgmat[4] = 0.0;
omgmat[7] = -invT[12];
omgmat[2] = -invT[13];
omgmat[5] = invT[12];
omgmat[8] = 0.0;
for (i = 0; i < 3; i++) {
d = omgmat[i + 3];
Fi = omgmat[i + 6];
for (i1 = 0; i1 < 3; i1++) {
b_AdTi_tmp = i1 << 2;
b_omgmat[i + 3 * i1] = (omgmat[i] * invT[b_AdTi_tmp] + d *
invT[b_AdTi_tmp + 1]) + Fi * invT[b_AdTi_tmp + 2];
b_invT[i1 + 6 * i] = invT[i1 + (i << 2)];
b_invT[i1 + 6 * (i + 3)] = 0.0;
}
}
for (i = 0; i < 3; i++) {
b_invT[6 * i + 3] = b_omgmat[3 * i];
invT_tmp = i << 2;
AdTi_tmp = 6 * (i + 3);
b_invT[AdTi_tmp + 3] = invT[invT_tmp];
b_invT[6 * i + 4] = b_omgmat[3 * i + 1];
b_invT[AdTi_tmp + 4] = invT[invT_tmp + 1];
b_invT[6 * i + 5] = b_omgmat[3 * i + 2];
b_invT[AdTi_tmp + 5] = invT[invT_tmp + 2];
}
for (i = 0; i < 6; i++) {
d = 0.0;
for (i1 = 0; i1 < 6; i1++) {
d += b_invT[i + 6 * i1] * Slist[i1 + 6 * b_i];
}
Ai[i + 6 * b_i] = d;
b_y[i] = d * -thetalist[b_i];
}
// *** 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
// *** 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
y[0] = 0.0;
y[4] = -b_y[2];
y[8] = b_y[1];
y[1] = b_y[2];
y[5] = 0.0;
y[9] = -b_y[0];
y[2] = -b_y[1];
y[6] = b_y[0];
y[10] = 0.0;
for (i = 0; i < 3; i++) {
AdTi_tmp = i + (b_i << 4);
omgmat[3 * i] = Mlist[AdTi_tmp];
omgmat[3 * i + 1] = Mlist[AdTi_tmp + 4];
omgmat[3 * i + 2] = Mlist[AdTi_tmp + 8];
y[i + 12] = b_y[i + 3];
}
y[3] = 0.0;
y[7] = 0.0;
y[11] = 0.0;
y[15] = 0.0;
MatrixExp6(y, invT);
for (i = 0; i < 9; i++) {
b_omgmat[i] = -omgmat[i];
}
i = b_i << 4;
for (i1 = 0; i1 < 3; i1++) {
AdTi_tmp = i1 << 2;
d_omgmat[AdTi_tmp] = omgmat[3 * i1];
d_omgmat[AdTi_tmp + 1] = omgmat[3 * i1 + 1];
d_omgmat[AdTi_tmp + 2] = omgmat[3 * i1 + 2];
d_omgmat[i1 + 12] = (b_omgmat[i1] * Mlist[i + 12] + b_omgmat[i1 + 3] *
Mlist[i + 13]) + b_omgmat[i1 + 6] * Mlist[i + 14];
}
d_omgmat[3] = 0.0;
d_omgmat[7] = 0.0;
d_omgmat[11] = 0.0;
d_omgmat[15] = 1.0;
for (i = 0; i < 4; i++) {
d = invT[i + 4];
Fi = invT[i + 8];
d1 = invT[i + 12];
for (i1 = 0; i1 < 4; i1++) {
b_AdTi_tmp = i1 << 2;
y[i + b_AdTi_tmp] = ((invT[i] * d_omgmat[b_AdTi_tmp] + d *
d_omgmat[b_AdTi_tmp + 1]) + Fi *
d_omgmat[b_AdTi_tmp + 2]) + d1 *
d_omgmat[b_AdTi_tmp + 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
omgmat[0] = 0.0;
omgmat[3] = -y[14];
omgmat[6] = y[13];
omgmat[1] = y[14];
omgmat[4] = 0.0;
omgmat[7] = -y[12];
omgmat[2] = -y[13];
omgmat[5] = y[12];
omgmat[8] = 0.0;
for (i = 0; i < 3; i++) {
d = omgmat[i + 3];
Fi = omgmat[i + 6];
for (i1 = 0; i1 < 3; i1++) {
b_AdTi_tmp = i1 << 2;
b_omgmat[i + 3 * i1] = (omgmat[i] * y[b_AdTi_tmp] + d * y[b_AdTi_tmp + 1])
+ Fi * y[b_AdTi_tmp + 2];
AdTi[(i1 + 6 * i) + 36 * b_i] = y[i1 + (i << 2)];
AdTi[(i1 + 6 * (i + 3)) + 36 * b_i] = 0.0;
}
}
for (i = 0; i < 3; i++) {
AdTi_tmp = 6 * i + 36 * b_i;
AdTi[AdTi_tmp + 3] = b_omgmat[3 * i];
invT_tmp = i << 2;
b_AdTi_tmp = 6 * (i + 3) + 36 * b_i;
AdTi[b_AdTi_tmp + 3] = y[invT_tmp];
AdTi[AdTi_tmp + 4] = b_omgmat[3 * i + 1];
AdTi[b_AdTi_tmp + 4] = y[invT_tmp + 1];
AdTi[AdTi_tmp + 5] = b_omgmat[3 * i + 2];
AdTi[b_AdTi_tmp + 5] = y[invT_tmp + 2];
}
for (i = 0; i < 6; i++) {
d = Ai[i + 6 * b_i] * 0.0;
b_y[i] = d;
Fi = 0.0;
for (i1 = 0; i1 < 6; i1++) {
Fi += AdTi[(i + 6 * i1) + 36 * b_i] * Vi[i1 + 6 * b_i];
}
b_AdTi[i] = Fi + d;
}
for (i = 0; i < 6; i++) {
Vi[i + 6 * (b_i + 1)] = b_AdTi[i];
}
// *** CHAPTER 8: DYNAMICS OF OPEN CHAINS ***
// Takes V: 6-vector spatial velocity.
// Returns adV: The corresponding 6x6 matrix.
// Used to calculate the Lie bracket [V1, V2] = [adV1]V2
// Example Input:
//
// clear; clc;
// V = [1; 2; 3; 4; 5; 6];
// adV = ad(V)
//
// Output:
// adV =
// 0 -3 2 0 0 0
// 3 0 -1 0 0 0
// -2 1 0 0 0 0
// 0 -6 5 0 -3 2
// 6 0 -4 3 0 -1
// -5 4 0 -2 1 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
omgmat[0] = 0.0;
i = 6 * (b_i + 1);
d = Vi[i + 2];
omgmat[3] = -d;
Fi = Vi[i + 1];
omgmat[6] = Fi;
omgmat[1] = d;
omgmat[4] = 0.0;
omgmat[7] = -Vi[i];
omgmat[2] = -Fi;
omgmat[5] = Vi[i];
omgmat[8] = 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_invT[3] = 0.0;
d = Vi[i + 5];
b_invT[9] = -d;
Fi = Vi[i + 4];
b_invT[15] = Fi;
b_invT[4] = d;
b_invT[10] = 0.0;
d = Vi[i + 3];
b_invT[16] = -d;
b_invT[5] = -Fi;
b_invT[11] = d;
b_invT[17] = 0.0;
for (i1 = 0; i1 < 3; i1++) {
d = omgmat[3 * i1];
b_invT[6 * i1] = d;
invT_tmp = 6 * (i1 + 3);
b_invT[invT_tmp] = 0.0;
b_invT[invT_tmp + 3] = d;
d = omgmat[3 * i1 + 1];
b_invT[6 * i1 + 1] = d;
b_invT[invT_tmp + 1] = 0.0;
b_invT[invT_tmp + 4] = d;
d = omgmat[3 * i1 + 2];
b_invT[6 * i1 + 2] = d;
b_invT[invT_tmp + 2] = 0.0;
b_invT[invT_tmp + 5] = d;
}
for (i1 = 0; i1 < 6; i1++) {
d = 0.0;
Fi = 0.0;
for (b_AdTi_tmp = 0; b_AdTi_tmp < 6; b_AdTi_tmp++) {
invT_tmp = i1 + 6 * b_AdTi_tmp;
AdTi_tmp = b_AdTi_tmp + 6 * b_i;
Fi += b_invT[invT_tmp] * Ai[AdTi_tmp];
d += AdTi[invT_tmp + 36 * b_i] * Vdi[AdTi_tmp];
}
b_AdTi[i1] = (d + b_y[i1]) + Fi * 0.0;
}
for (i1 = 0; i1 < 6; i1++) {
Vdi[i1 + i] = b_AdTi[i1];
}
if (*emlrtBreakCheckR2012bFlagVar != 0) {
emlrtBreakCheckR2012b(sp);
}
}
omgmat[0] = 0.0;
omgmat[4] = 0.0;
omgmat[8] = 0.0;
for (b_i = 0; b_i < 9; b_i++) {
// *** CHAPTER 8: DYNAMICS OF OPEN CHAINS ***
// Takes V: 6-vector spatial velocity.
// Returns adV: The corresponding 6x6 matrix.
// Used to calculate the Lie bracket [V1, V2] = [adV1]V2
// Example Input:
//
// clear; clc;
// V = [1; 2; 3; 4; 5; 6];
// adV = ad(V)
//
// Output:
// adV =
// 0 -3 2 0 0 0
// 3 0 -1 0 0 0
// -2 1 0 0 0 0
// 0 -6 5 0 -3 2
// 6 0 -4 3 0 -1
// -5 4 0 -2 1 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
i = 6 * (9 - b_i);
d = Vi[i + 2];
omgmat[3] = -d;
Fi = Vi[i + 1];
omgmat[6] = Fi;
omgmat[1] = d;
omgmat[7] = -Vi[i];
omgmat[2] = -Fi;
omgmat[5] = Vi[i];
// *** 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
for (i1 = 0; i1 < 6; i1++) {
d = 0.0;
Fi = 0.0;
for (b_AdTi_tmp = 0; b_AdTi_tmp < 6; b_AdTi_tmp++) {
d += AdTi[(b_AdTi_tmp + 6 * i1) + 36 * (9 - b_i)] * b_Fi[b_AdTi_tmp];
Fi += Glist_data[(i1 + 6 * b_AdTi_tmp) + 36 * (8 - b_i)] *
Vdi[b_AdTi_tmp + i];
}
b_y[i1] = Fi;
b_AdTi[i1] = d;
}
b_invT[18] = 0.0;
d = Vi[i + 5];
b_invT[19] = -d;
Fi = Vi[i + 4];
b_invT[20] = Fi;
b_invT[24] = d;
b_invT[25] = 0.0;
d = Vi[i + 3];
b_invT[26] = -d;
b_invT[30] = -Fi;
b_invT[31] = d;
b_invT[32] = 0.0;
for (i1 = 0; i1 < 3; i1++) {
b_invT[6 * i1] = omgmat[i1];
b_invT[6 * i1 + 3] = 0.0;
invT_tmp = 6 * (i1 + 3);
b_invT[invT_tmp + 3] = omgmat[i1];
d = omgmat[i1 + 3];
b_invT[6 * i1 + 1] = d;
b_invT[6 * i1 + 4] = 0.0;
b_invT[invT_tmp + 4] = d;
d = omgmat[i1 + 6];
b_invT[6 * i1 + 2] = d;
b_invT[6 * i1 + 5] = 0.0;
b_invT[invT_tmp + 5] = d;
}
for (i1 = 0; i1 < 6; i1++) {
d = 0.0;
for (b_AdTi_tmp = 0; b_AdTi_tmp < 6; b_AdTi_tmp++) {
d += Glist_data[(i1 + 6 * b_AdTi_tmp) + 36 * (8 - b_i)] * Vi[b_AdTi_tmp
+ i];
}
c_omgmat[i1] = d;
}
// Fi = Glist(:, :, i) * Vdi(:, i + 1) ;
Fi = 0.0;
for (i = 0; i < 6; i++) {
d = 0.0;
for (i1 = 0; i1 < 6; i1++) {
d += b_invT[i + 6 * i1] * c_omgmat[i1];
}
d = (b_AdTi[i] + b_y[i]) - d;
b_Fi[i] = d;
Flist[(9 * i - b_i) + 8] = d;
Fi += d * Ai[i + 6 * (8 - b_i)];
}
taulist[8 - b_i] = Fi;
if (*emlrtBreakCheckR2012bFlagVar != 0) {
emlrtBreakCheckR2012b(sp);
}
}
}
// End of code generation (InverseDynamics_debug.cpp)
Reference in New Issue View Git Blame Copy Permalink