File: calculateTransformations.c

    1   /*
    2    * Academic License - for use in teaching, academic research, and meeting
    3    * course requirements at degree granting institutions only.  Not for
    4    * government, commercial, or other organizational use.
    5    *
    6    * calculateTransformations.c
    7    *
    8    * Code generation for function 'calculateTransformations'
    9    *
   10    */
   11   
   12   /* Include files */
   13   #include "rt_nonfinite.h"
   14   #include "calculateTransformations.h"
   15   #include "rot2euler.h"
   16   #include "calculateTransformations_data.h"
   17   
   18   /* Variable Definitions */
   19   static emlrtRSInfo emlrtRSI = { 29, "calculateTransformations",
   20     "C:\\Users\\jorgesv\\Dropbox\\PhD documents\\Matlab code\\underwater swimming manipulators\\Utilities\\calculateTransformations.m"
   21   };
   22   
   23   static emlrtRSInfo b_emlrtRSI = { 42, "calculateTransformations",
   24     "C:\\Users\\jorgesv\\Dropbox\\PhD documents\\Matlab code\\underwater swimming manipulators\\Utilities\\calculateTransformations.m"
   25   };
   26   
   27   static emlrtRSInfo c_emlrtRSI = { 45, "calculateTransformations",
   28     "C:\\Users\\jorgesv\\Dropbox\\PhD documents\\Matlab code\\underwater swimming manipulators\\Utilities\\calculateTransformations.m"
   29   };
   30   
   31   static emlrtRSInfo d_emlrtRSI = { 48, "calculateTransformations",
   32     "C:\\Users\\jorgesv\\Dropbox\\PhD documents\\Matlab code\\underwater swimming manipulators\\Utilities\\calculateTransformations.m"
   33   };
   34   
   35   static emlrtRSInfo e_emlrtRSI = { 74, "calculateTransformations",
   36     "C:\\Users\\jorgesv\\Dropbox\\PhD documents\\Matlab code\\underwater swimming manipulators\\Utilities\\calculateTransformations.m"
   37   };
   38   
   39   static emlrtBCInfo emlrtBCI = { 1, 8, 16, 42, "g_joints",
   40     "calculateModuleTransformations",
   41     "C:\\Users\\jorgesv\\Dropbox\\PhD documents\\Matlab code\\underwater swimming manipulators\\Utilities\\calculateModuleTransformations.m",
   42     0 };
   43   
   44   static emlrtBCInfo b_emlrtBCI = { 1, 8, 30, 45, "g_joints",
   45     "calculateJointTransformations",
   46     "C:\\Users\\jorgesv\\Dropbox\\PhD documents\\Matlab code\\underwater swimming manipulators\\Utilities\\calculateJointTransformations.m",
   47     0 };
   48   
   49   static emlrtBCInfo c_emlrtBCI = { 1, 8, 29, 40, "g_joints",
   50     "calculateJointTransformations",
   51     "C:\\Users\\jorgesv\\Dropbox\\PhD documents\\Matlab code\\underwater swimming manipulators\\Utilities\\calculateJointTransformations.m",
   52     0 };
   53   
   54   /* Function Definitions */
   55   void calculateTransformations(const emlrtStack *sp, const real_T q1[5], const
   56     real_T q2[5], const real_T eta_marker[6], const real_T g_marker2front[16],
   57     real_T controlCenterLink, real_T g_If[16], real_T R_If[9], real_T eta_f[6],
   58     real_T g_Ib[16], real_T R_Ib[9], real_T eta_b[6], real_T q[8], real_T
   59     g_joints_home[144], real_T g_moduleCM[144], real_T g_thrusters[112])
   60   {
   61     real_T cphi;
   62     real_T sphi;
   63     real_T cth;
   64     real_T sth;
   65     real_T cpsi;
   66     real_T spsi;
   67     real_T b_cpsi[16];
   68     int32_T k;
   69     static const int8_T iv0[4] = { 0, 0, 0, 1 };
   70   
   71     int32_T i0;
   72     int32_T i1;
   73     real_T b_g_If[9];
   74     real_T b_g_joints_home[3];
   75     real_T g_joints[128];
   76     int32_T i;
   77     boolean_T b_bool;
   78     static const char_T joint_ax[8] = { 'Z', 'Y', 'Z', 'Y', 'Z', 'Y', 'Z', 'Y' };
   79   
   80     real_T cos_angle;
   81     real_T sin_angle;
   82     real_T g_front2back[16];
   83     static const int8_T iv1[3] = { 0, 1, 0 };
   84   
   85     static const real_T joint_tr[27] = { 0.62, 0.1, 0.59, 0.1, 0.8, 0.1, 0.59, 0.1,
   86       0.37, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
   87       0.0, 0.0, 0.0, 0.0 };
   88   
   89     int8_T I[9];
   90     real_T g_center2back[16];
   91     static const int8_T iv2[3] = { 0, 0, 1 };
   92   
   93     static const real_T g_ne[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
   94       0.0, 1.0, 0.0, 0.37, 0.0, 0.0, 1.0 };
   95   
   96     static const int8_T iv3[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
   97   
   98     static const real_T module_cm[27] = { 0.31, 0.05, 0.295, 0.05, 0.4, 0.05,
   99       0.295, 0.05, 0.185, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
  100       0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
  101   
  102     static const char_T thruster_ax[7] = { 'Z', 'Y', 'Z', 'X', 'X', 'Y', 'Z' };
  103   
  104     static const real_T rotZ[9] = { 6.123233995736766E-17, 1.0, 0.0, -1.0,
  105       6.123233995736766E-17, 0.0, 0.0, 0.0, 1.0 };
  106   
  107     static const real_T thruster_pos[21] = { 0.24, 0.35000000000000003, 0.24, 0.53,
  108       0.53, 0.24, 0.35000000000000003, 0.0, 0.0, 0.0, 0.15, -0.15, 0.0, 0.0, 0.0,
  109       0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
  110   
  111     static const real_T rotY[9] = { 6.123233995736766E-17, 0.0, 1.0, 0.0, 1.0, 0.0,
  112       -1.0, 0.0, 6.123233995736766E-17 };
  113   
  114     static const int8_T iv4[7] = { 3, 3, 5, 5, 5, 7, 7 };
  115   
  116     emlrtStack st;
  117     st.prev = sp;
  118     st.tls = sp->tls;
  119   
  120     /*  [EELY] Specific code for Eely */
  121     /*  Measured: Position and Euler angles for the markers, eta_marker */
  122     /*  R = Rzyx(phi,theta,psi) computes the Euler angle */
  123     /*  rotation matrix R in SO(3) using the zyx convention */
  124     /*  */
  125     /*  Author:   Thor I. Fossen */
  126     /*  Date:     14th June 2001 */
  127     /*  Revisions:  */
  128     /*  ________________________________________________________________ */
  129     /*  */
  130     /*  MSS GNC is a Matlab toolbox for guidance, navigation and control. */
  131     /*  The toolbox is part of the Marine Systems Simulator (MSS). */
  132     /*  */
  133     /*  Copyright (C) 2008 Thor I. Fossen and Tristan Perez */
  134     /*   */
  135     /*  This program is free software: you can redistribute it and/or modify */
  136     /*  it under the terms of the GNU General Public License as published by */
  137     /*  the Free Software Foundation, either version 3 of the License, or */
  138     /*  (at your option) any later version. */
  139     /*   */
  140     /*  This program is distributed in the hope that it will be useful, but */
  141     /*  WITHOUT ANY WARRANTY; without even the implied warranty of */
  142     /*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the  */
  143     /*  GNU General Public License for more details. */
  144     /*   */
  145     /*  You should have received a copy of the GNU General Public License */
  146     /*  along with this program.  If not, see <http://www.gnu.org/licenses/>. */
  147     /*   */
  148     /*  E-mail: contact@marinecontrol.org */
  149     /*  URL:    <http://www.marinecontrol.org> */
  150     cphi = muDoubleScalarCos(eta_marker[3]);
  151     sphi = muDoubleScalarSin(eta_marker[3]);
  152     cth = muDoubleScalarCos(eta_marker[4]);
  153     sth = muDoubleScalarSin(eta_marker[4]);
  154     cpsi = muDoubleScalarCos(eta_marker[5]);
  155     spsi = muDoubleScalarSin(eta_marker[5]);
  156     b_cpsi[0] = cpsi * cth;
  157     b_cpsi[4] = -spsi * cphi + cpsi * sth * sphi;
  158     b_cpsi[8] = spsi * sphi + cpsi * cphi * sth;
  159     b_cpsi[1] = spsi * cth;
  160     b_cpsi[5] = cpsi * cphi + sphi * sth * spsi;
  161     b_cpsi[9] = -cpsi * sphi + sth * spsi * cphi;
  162     b_cpsi[2] = -sth;
  163     b_cpsi[6] = cth * sphi;
  164     b_cpsi[10] = cth * cphi;
  165     for (k = 0; k < 3; k++) {
  166       b_cpsi[12 + k] = eta_marker[k];
  167     }
  168   
  169     for (k = 0; k < 4; k++) {
  170       b_cpsi[3 + (k << 2)] = iv0[k];
  171     }
  172   
  173     for (k = 0; k < 4; k++) {
  174       for (i0 = 0; i0 < 4; i0++) {
  175         g_If[k + (i0 << 2)] = 0.0;
  176         for (i1 = 0; i1 < 4; i1++) {
  177           g_If[k + (i0 << 2)] += b_cpsi[k + (i1 << 2)] * g_marker2front[i1 + (i0 <<
  178             2)];
  179         }
  180       }
  181     }
  182   
  183     for (k = 0; k < 3; k++) {
  184       for (i0 = 0; i0 < 3; i0++) {
  185         R_If[i0 + 3 * k] = g_If[i0 + (k << 2)];
  186         b_g_If[i0 + 3 * k] = g_If[i0 + (k << 2)];
  187       }
  188     }
  189   
  190     st.site = &emlrtRSI;
  191     rot2euler(&st, b_g_If, b_g_joints_home);
  192     for (k = 0; k < 3; k++) {
  193       eta_f[k] = g_If[12 + k];
  194       eta_f[k + 3] = b_g_joints_home[k];
  195     }
  196   
  197     /*  Code to convert the joint angle inputs q1 and q2 from the Eely robot */
  198     /*  to the definition used in this code by reorganizing the vector elements. */
  199     q[0] = q2[3] * 3.1415926535897931 / 180.0;
  200     q[1] = q1[3] * 3.1415926535897931 / 180.0;
  201     q[2] = q2[2] * 3.1415926535897931 / 180.0;
  202     q[3] = q1[2] * 3.1415926535897931 / 180.0;
  203     q[4] = q2[1] * 3.1415926535897931 / 180.0;
  204     q[5] = q1[1] * 3.1415926535897931 / 180.0;
  205     q[6] = q2[4] * 3.1415926535897931 / 180.0;
  206     q[7] = q1[4] * 3.1415926535897931 / 180.0;
  207   
  208     /* % TRANSFORMATIONS */
  209     /*  Calculate the sequential transformations from base frame to the joint */
  210     /*  frames */
  211     st.site = &b_emlrtRSI;
  212   
  213     /*  Calculates the transformation matrices from  */
  214     /*  the USM base frame, F_b = F_0, to the joint frames F_i (i=1..n) */
  215     /*  From the tail module and forward */
  216     /*  JOINT FRAMES */
  217     memset(&g_joints[0], 0, sizeof(real_T) << 7);
  218     memset(&g_joints_home[0], 0, 144U * sizeof(real_T));
  219   
  220     /*  size = num_joint+1 to include F_e */
  221     i = 0;
  222     while (i < 8) {
  223       b_bool = false;
  224       if (joint_ax[i] != 'Y') {
  225       } else {
  226         b_bool = true;
  227       }
  228   
  229       if (b_bool) {
  230         cos_angle = muDoubleScalarCos(q[i]);
  231         sin_angle = muDoubleScalarSin(q[i]);
  232         g_front2back[0] = cos_angle;
  233         g_front2back[4] = 0.0;
  234         g_front2back[8] = sin_angle;
  235         g_front2back[2] = -sin_angle;
  236         g_front2back[6] = 0.0;
  237         g_front2back[10] = cos_angle;
  238         for (k = 0; k < 3; k++) {
  239           g_front2back[1 + (k << 2)] = iv1[k];
  240           g_front2back[12 + k] = joint_tr[i + 9 * k];
  241         }
  242   
  243         for (k = 0; k < 4; k++) {
  244           g_front2back[3 + (k << 2)] = iv0[k];
  245         }
  246   
  247         for (k = 0; k < 9; k++) {
  248           I[k] = 0;
  249         }
  250   
  251         for (k = 0; k < 3; k++) {
  252           I[k + 3 * k] = 1;
  253         }
  254   
  255         for (k = 0; k < 3; k++) {
  256           for (i0 = 0; i0 < 3; i0++) {
  257             g_center2back[i0 + (k << 2)] = I[i0 + 3 * k];
  258           }
  259   
  260           g_center2back[12 + k] = joint_tr[i + 9 * k];
  261         }
  262   
  263         for (k = 0; k < 4; k++) {
  264           g_center2back[3 + (k << 2)] = iv0[k];
  265         }
  266       } else {
  267         b_bool = false;
  268         if (joint_ax[i] != 'Z') {
  269         } else {
  270           b_bool = true;
  271         }
  272   
  273         if (b_bool) {
  274           cos_angle = muDoubleScalarCos(q[i]);
  275           sin_angle = muDoubleScalarSin(q[i]);
  276           g_front2back[0] = cos_angle;
  277           g_front2back[4] = -sin_angle;
  278           g_front2back[8] = 0.0;
  279           g_front2back[1] = sin_angle;
  280           g_front2back[5] = cos_angle;
  281           g_front2back[9] = 0.0;
  282           for (k = 0; k < 3; k++) {
  283             g_front2back[2 + (k << 2)] = iv2[k];
  284             g_front2back[12 + k] = joint_tr[i + 9 * k];
  285           }
  286   
  287           for (k = 0; k < 4; k++) {
  288             g_front2back[3 + (k << 2)] = iv0[k];
  289           }
  290   
  291           for (k = 0; k < 9; k++) {
  292             I[k] = 0;
  293           }
  294   
  295           for (k = 0; k < 3; k++) {
  296             I[k + 3 * k] = 1;
  297           }
  298   
  299           for (k = 0; k < 3; k++) {
  300             for (i0 = 0; i0 < 3; i0++) {
  301               g_center2back[i0 + (k << 2)] = I[i0 + 3 * k];
  302             }
  303   
  304             g_center2back[12 + k] = joint_tr[i + 9 * k];
  305           }
  306   
  307           for (k = 0; k < 4; k++) {
  308             g_center2back[3 + (k << 2)] = iv0[k];
  309           }
  310         } else {
  311           memset(&g_front2back[0], 0, sizeof(real_T) << 4);
  312           for (k = 0; k < 4; k++) {
  313             g_front2back[k + (k << 2)] = 1.0;
  314           }
  315   
  316           memset(&g_center2back[0], 0, sizeof(real_T) << 4);
  317           for (k = 0; k < 4; k++) {
  318             g_center2back[k + (k << 2)] = 1.0;
  319           }
  320         }
  321       }
  322   
  323       if (1 + i == 1) {
  324         for (k = 0; k < 4; k++) {
  325           for (i0 = 0; i0 < 4; i0++) {
  326             g_joints[(i0 + (k << 2)) + (i << 4)] = g_front2back[i0 + (k << 2)];
  327             g_joints_home[(i0 + (k << 2)) + (i << 4)] = g_center2back[i0 + (k << 2)];
  328           }
  329         }
  330       } else {
  331         k = i;
  332         if (!(k >= 1)) {
  333           emlrtDynamicBoundsCheckR2012b(0, 1, 8, &c_emlrtBCI, &st);
  334         }
  335   
  336         for (k = 0; k < 4; k++) {
  337           for (i0 = 0; i0 < 4; i0++) {
  338             b_cpsi[k + (i0 << 2)] = 0.0;
  339             for (i1 = 0; i1 < 4; i1++) {
  340               b_cpsi[k + (i0 << 2)] += g_joints[(k + (i1 << 2)) + ((i - 1) << 4)] *
  341                 g_front2back[i1 + (i0 << 2)];
  342             }
  343           }
  344         }
  345   
  346         for (k = 0; k < 4; k++) {
  347           for (i0 = 0; i0 < 4; i0++) {
  348             g_joints[(i0 + (k << 2)) + (i << 4)] = b_cpsi[i0 + (k << 2)];
  349           }
  350         }
  351   
  352         k = i;
  353         if (!(k >= 1)) {
  354           emlrtDynamicBoundsCheckR2012b(0, 1, 8, &b_emlrtBCI, &st);
  355         }
  356   
  357         for (k = 0; k < 4; k++) {
  358           for (i0 = 0; i0 < 4; i0++) {
  359             g_joints_home[(k + (i0 << 2)) + (i << 4)] = 0.0;
  360             for (i1 = 0; i1 < 4; i1++) {
  361               g_joints_home[(k + (i0 << 2)) + (i << 4)] += g_joints[(k + (i1 << 2))
  362                 + ((i - 1) << 4)] * g_center2back[i1 + (i0 << 2)];
  363             }
  364           }
  365         }
  366       }
  367   
  368       i++;
  369       if (*emlrtBreakCheckR2012bFlagVar != 0) {
  370         emlrtBreakCheckR2012b(&st);
  371       }
  372     }
  373   
  374     for (k = 0; k < 4; k++) {
  375       for (i0 = 0; i0 < 4; i0++) {
  376         g_joints_home[128 + (k + (i0 << 2))] = 0.0;
  377         for (i1 = 0; i1 < 4; i1++) {
  378           g_joints_home[128 + (k + (i0 << 2))] += g_joints[112 + (k + (i1 << 2))] *
  379             g_ne[i1 + (i0 << 2)];
  380         }
  381       }
  382     }
  383   
  384     /*  Calculate transformations for the CM of each module */
  385     st.site = &c_emlrtRSI;
  386   
  387     /*  Calculating the transformations from the USM base frame, F_b = F_0, */
  388     /*  to the MODULE CENTER OF MASS FRAMES, F_CM_i */
  389     /*  From the tail module and forward */
  390     memset(&g_moduleCM[0], 0, 144U * sizeof(real_T));
  391     i = 0;
  392     while (i < 9) {
  393       for (k = 0; k < 3; k++) {
  394         for (i0 = 0; i0 < 3; i0++) {
  395           g_front2back[i0 + (k << 2)] = iv3[i0 + 3 * k];
  396         }
  397   
  398         g_front2back[12 + k] = module_cm[i + 9 * k];
  399       }
  400   
  401       for (k = 0; k < 4; k++) {
  402         g_front2back[3 + (k << 2)] = iv0[k];
  403       }
  404   
  405       if (1 + i == 1) {
  406         for (k = 0; k < 4; k++) {
  407           for (i0 = 0; i0 < 4; i0++) {
  408             g_moduleCM[(i0 + (k << 2)) + (i << 4)] = g_front2back[i0 + (k << 2)];
  409           }
  410         }
  411       } else {
  412         k = i;
  413         if (!(k >= 1)) {
  414           emlrtDynamicBoundsCheckR2012b(0, 1, 8, &emlrtBCI, &st);
  415         }
  416   
  417         for (k = 0; k < 4; k++) {
  418           for (i0 = 0; i0 < 4; i0++) {
  419             g_moduleCM[(k + (i0 << 2)) + (i << 4)] = 0.0;
  420             for (i1 = 0; i1 < 4; i1++) {
  421               g_moduleCM[(k + (i0 << 2)) + (i << 4)] += g_joints[(k + (i1 << 2)) +
  422                 ((i - 1) << 4)] * g_front2back[i1 + (i0 << 2)];
  423             }
  424           }
  425         }
  426       }
  427   
  428       i++;
  429       if (*emlrtBreakCheckR2012bFlagVar != 0) {
  430         emlrtBreakCheckR2012b(&st);
  431       }
  432     }
  433   
  434     /*  Calculate transformations for the thrusters */
  435     st.site = &d_emlrtRSI;
  436   
  437     /*  Calculating the transformations from the back frame to the thruster */
  438     /*  frames */
  439     /*  From the tail module and forward */
  440     /*  THRUSTER FRAMES */
  441     memset(&g_thrusters[0], 0, 112U * sizeof(real_T));
  442     i = 0;
  443     while (i < 7) {
  444       b_bool = false;
  445       if (thruster_ax[i] != 'Y') {
  446       } else {
  447         b_bool = true;
  448       }
  449   
  450       if (b_bool) {
  451         k = 0;
  452       } else {
  453         b_bool = false;
  454         if (thruster_ax[i] != 'Z') {
  455         } else {
  456           b_bool = true;
  457         }
  458   
  459         if (b_bool) {
  460           k = 1;
  461         } else {
  462           k = -1;
  463         }
  464       }
  465   
  466       switch (k) {
  467        case 0:
  468         for (k = 0; k < 3; k++) {
  469           for (i0 = 0; i0 < 3; i0++) {
  470             g_front2back[i0 + (k << 2)] = rotZ[i0 + 3 * k];
  471           }
  472   
  473           g_front2back[12 + k] = thruster_pos[i + 7 * k];
  474         }
  475   
  476         for (k = 0; k < 4; k++) {
  477           g_front2back[3 + (k << 2)] = iv0[k];
  478         }
  479         break;
  480   
  481        case 1:
  482         for (k = 0; k < 3; k++) {
  483           for (i0 = 0; i0 < 3; i0++) {
  484             g_front2back[i0 + (k << 2)] = rotY[i0 + 3 * k];
  485           }
  486   
  487           g_front2back[12 + k] = thruster_pos[i + 7 * k];
  488         }
  489   
  490         for (k = 0; k < 4; k++) {
  491           g_front2back[3 + (k << 2)] = iv0[k];
  492         }
  493         break;
  494   
  495        default:
  496         memset(&g_front2back[0], 0, sizeof(real_T) << 4);
  497         for (k = 0; k < 4; k++) {
  498           g_front2back[k + (k << 2)] = 1.0;
  499         }
  500         break;
  501       }
  502   
  503       for (k = 0; k < 4; k++) {
  504         for (i0 = 0; i0 < 4; i0++) {
  505           g_center2back[i0 + (k << 2)] = g_joints[(i0 + (k << 2)) + ((iv4[i] - 2) <<
  506             4)];
  507         }
  508       }
  509   
  510       for (k = 0; k < 4; k++) {
  511         for (i0 = 0; i0 < 4; i0++) {
  512           g_thrusters[(k + (i0 << 2)) + (i << 4)] = 0.0;
  513           for (i1 = 0; i1 < 4; i1++) {
  514             g_thrusters[(k + (i0 << 2)) + (i << 4)] += g_center2back[k + (i1 << 2)]
  515               * g_front2back[i1 + (i0 << 2)];
  516           }
  517         }
  518       }
  519   
  520       i++;
  521       if (*emlrtBreakCheckR2012bFlagVar != 0) {
  522         emlrtBreakCheckR2012b(&st);
  523       }
  524     }
  525   
  526     /*  Calculate the transformation from the front end to the back end */
  527     for (k = 0; k < 3; k++) {
  528       for (i0 = 0; i0 < 3; i0++) {
  529         b_g_If[i0 + 3 * k] = -g_joints_home[128 + (k + (i0 << 2))];
  530       }
  531     }
  532   
  533     for (k = 0; k < 3; k++) {
  534       b_g_joints_home[k] = 0.0;
  535       for (i0 = 0; i0 < 3; i0++) {
  536         g_front2back[i0 + (k << 2)] = g_joints_home[128 + (k + (i0 << 2))];
  537         b_g_joints_home[k] += b_g_If[k + 3 * i0] * g_joints_home[140 + i0];
  538       }
  539   
  540       g_front2back[12 + k] = b_g_joints_home[k];
  541     }
  542   
  543     for (k = 0; k < 4; k++) {
  544       g_front2back[3 + (k << 2)] = iv0[k];
  545     }
  546   
  547     /*  Transformation from inertial frame to base frame */
  548     if (controlCenterLink != 0.0) {
  549       /*  Calculate the transformation from the back end to the center module */
  550       /*  (module number 5) */
  551       for (k = 0; k < 3; k++) {
  552         for (i0 = 0; i0 < 3; i0++) {
  553           b_g_If[i0 + 3 * k] = -g_moduleCM[64 + (k + (i0 << 2))];
  554         }
  555       }
  556   
  557       for (k = 0; k < 3; k++) {
  558         b_g_joints_home[k] = 0.0;
  559         for (i0 = 0; i0 < 3; i0++) {
  560           g_center2back[i0 + (k << 2)] = g_moduleCM[64 + (k + (i0 << 2))];
  561           b_g_joints_home[k] += b_g_If[k + 3 * i0] * g_moduleCM[76 + i0];
  562         }
  563   
  564         g_center2back[12 + k] = b_g_joints_home[k];
  565       }
  566   
  567       for (k = 0; k < 4; k++) {
  568         g_center2back[3 + (k << 2)] = iv0[k];
  569         for (i0 = 0; i0 < 4; i0++) {
  570           b_cpsi[k + (i0 << 2)] = 0.0;
  571           for (i1 = 0; i1 < 4; i1++) {
  572             b_cpsi[k + (i0 << 2)] += g_If[k + (i1 << 2)] * g_front2back[i1 + (i0 <<
  573               2)];
  574           }
  575         }
  576   
  577         for (i0 = 0; i0 < 4; i0++) {
  578           g_Ib[k + (i0 << 2)] = 0.0;
  579           for (i1 = 0; i1 < 4; i1++) {
  580             g_Ib[k + (i0 << 2)] += b_cpsi[k + (i1 << 2)] * g_moduleCM[64 + (i1 +
  581               (i0 << 2))];
  582           }
  583         }
  584       }
  585   
  586       i = 0;
  587       while (i < 7) {
  588         for (k = 0; k < 4; k++) {
  589           for (i0 = 0; i0 < 4; i0++) {
  590             b_cpsi[k + (i0 << 2)] = 0.0;
  591             for (i1 = 0; i1 < 4; i1++) {
  592               b_cpsi[k + (i0 << 2)] += g_center2back[k + (i1 << 2)] * g_thrusters
  593                 [(i1 + (i0 << 2)) + (i << 4)];
  594             }
  595           }
  596         }
  597   
  598         for (k = 0; k < 4; k++) {
  599           for (i0 = 0; i0 < 4; i0++) {
  600             g_thrusters[(i0 + (k << 2)) + (i << 4)] = b_cpsi[i0 + (k << 2)];
  601           }
  602         }
  603   
  604         i++;
  605         if (*emlrtBreakCheckR2012bFlagVar != 0) {
  606           emlrtBreakCheckR2012b(sp);
  607         }
  608       }
  609   
  610       i = 0;
  611       while (i < 9) {
  612         for (k = 0; k < 4; k++) {
  613           for (i0 = 0; i0 < 4; i0++) {
  614             b_cpsi[k + (i0 << 2)] = 0.0;
  615             for (i1 = 0; i1 < 4; i1++) {
  616               b_cpsi[k + (i0 << 2)] += g_center2back[k + (i1 << 2)] * g_moduleCM
  617                 [(i1 + (i0 << 2)) + (i << 4)];
  618             }
  619           }
  620         }
  621   
  622         for (k = 0; k < 4; k++) {
  623           for (i0 = 0; i0 < 4; i0++) {
  624             g_moduleCM[(i0 + (k << 2)) + (i << 4)] = b_cpsi[i0 + (k << 2)];
  625           }
  626         }
  627   
  628         i++;
  629         if (*emlrtBreakCheckR2012bFlagVar != 0) {
  630           emlrtBreakCheckR2012b(sp);
  631         }
  632       }
  633     } else {
  634       for (k = 0; k < 4; k++) {
  635         for (i0 = 0; i0 < 4; i0++) {
  636           g_Ib[k + (i0 << 2)] = 0.0;
  637           for (i1 = 0; i1 < 4; i1++) {
  638             g_Ib[k + (i0 << 2)] += g_If[k + (i1 << 2)] * g_front2back[i1 + (i0 <<
  639               2)];
  640           }
  641         }
  642       }
  643     }
  644   
  645     for (k = 0; k < 3; k++) {
  646       for (i0 = 0; i0 < 3; i0++) {
  647         R_Ib[i0 + 3 * k] = g_Ib[i0 + (k << 2)];
  648         b_g_If[i0 + 3 * k] = g_Ib[i0 + (k << 2)];
  649       }
  650     }
  651   
  652     st.site = &e_emlrtRSI;
  653     rot2euler(&st, b_g_If, b_g_joints_home);
  654     for (k = 0; k < 3; k++) {
  655       eta_b[k] = g_Ib[12 + k];
  656       eta_b[k + 3] = b_g_joints_home[k];
  657     }
  658   }
  659   
  660   /* End of code generation (calculateTransformations.c) */
  661