//
//  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)