# EOM.py: Define equation of motion of falling cat
# 
# input
#  para: torque at joint 
#        T = T1*a1^ + T2*a2^ + T3*a3^
#        i.e. array([[T1, T2, T3]])
#  state: list of state variables
#         q1, ..., q9, u1, ..., u9
#         i.e. array([[q1, ..., q9, u1, ..., u9]])
#
# output
#  delta variables: calculated delta state variables
#


# import packages
from numpy import array
from scipy import *

#def EOM(para, state):  # input: parameter & initial state
def EOM(state, t, para):

    # define initial state & parameter
    q1, q2, q3, q4, q5, q6, q7, q8, q9, u1, u2, u3 ,u4, u5, u6, u7, u8, u9 = state      
    T1, T2, T3 = para

    # define constants
    m = 2.5  # mass of a cylinder [kg]
    R = 0.075  # radius of a cylinder [m]
    L = 0.25  # length of a cylinder [m]
    g = 9.80665  # gravity [m/s^2]

    # define inertia tensor
    I1 = 0.5 * m * R ** 2
    I2 = 1.0 / 12.0 * m * (L ** 2 + 3 * R ** 2)
    I3 = I2

    # Reserve space and initialize matrices
    z = zeros((661,1))

    z[145] = L*m
    z[206] = g*m
    z[207] = L*z[206]
    z[234] = I2 + 0.25*m*L**2
    z[235] = m*L**2
    z[239] = I3 + 0.25*m*L**2
    z[254] = m**2
    z[255] = m*z[234]
    z[257] = z[234]*z[239]
    z[364] = z[257]**2
    z[457] = m*z[239]
    z[556] = z[239]**2

    q1p = u1
    q2p = u2
    q3p = u3

    z[1] = cos(q5);
    z[2] = cos(q6);
    z[3] = z[1]*z[2];
    z[6] = sin(q5);
    z[4] = sin(q6);
    z[19] = z[4]*z[6];
    z[8] = sin(q4);
    z[11] = z[1]*z[8];
    z[22] = z[1]*z[4]*z[8];
    z[7] = cos(q4);
    z[14] = z[1]*z[7];
    z[23] = z[1]*z[4]*z[7];
    z[26] = z[6]*z[19] + z[11]*z[22] + z[14]*z[23];
    z[5] = z[1]*z[4];
    z[15] = z[2]*z[6];
    z[10] = z[2]*z[7] - z[4]*z[6]*z[8];
    z[17] = z[1]*z[2]*z[8];
    z[13] = z[2]*z[8] + z[4]*z[6]*z[7];
    z[18] = z[1]*z[2]*z[7];
    z[31] = z[5]*z[15] + z[10]*z[17] - z[13]*z[18];
    z[9] = z[4]*z[7] + z[2]*z[6]*z[8];
    z[24] = z[6]*z[8];
    z[12] = z[4]*z[8] - z[2]*z[6]*z[7];
    z[25] = z[6]*z[7];
    z[29] = z[1]*z[3] + z[9]*z[24] - z[12]*z[25];
    z[77] = z[3]*z[26] + z[6]*z[31] - z[5]*z[29];
    z[16] = z[2]*z[6]*z[7] - z[4]*z[8];
    z[21] = -z[4]*z[7] - z[2]*z[6]*z[8];
    z[28] = z[14]*z[16] - z[3]*z[6] - z[11]*z[21];
    z[32] = z[5]**2 + z[10]**2 + z[13]**2;
    z[81] = z[9]*z[28] - z[11]*z[32];
    z[78] = z[3]*z[28] + z[6]*z[32];
    z[80] = z[9]*z[26] + z[10]*z[29] - z[11]*z[31];
    z[96] = z[77]*z[81] - z[78]*z[80];
    z[84] = z[12]*z[28] + z[14]*z[32];
    z[83] = z[12]*z[26] + z[13]*z[29] + z[14]*z[31];
    z[98] = z[80]*z[84] - z[81]*z[83];
    z[101] = z[77]*z[84] - z[78]*z[83];
    z[20] = -z[2]*z[8] - z[4]*z[6]*z[7];
    z[27] = z[10]*z[14] - z[11]*z[20];
    z[30] = -z[9]*z[14] - z[11]*z[12];
    z[33] = z[9]*z[13] + z[10]*z[16];
    z[82] = z[12]*z[27] + z[13]*z[30] + z[14]*z[33];
    z[76] = z[3]*z[27] + z[6]*z[33] - z[5]*z[30];
    z[79] = z[9]*z[27] + z[10]*z[30] - z[11]*z[33];
    z[94] = z[76]*z[80] - z[77]*z[79];
    z[95] = z[76]*z[81] - z[78]*z[79];
    z[97] = z[82]*z[96] + z[84]*z[94] - z[83]*z[95];
    z[104] = (z[96]*u6+z[98]*u4-z[101]*u5)/z[97];
    q4p = z[104];
    z[99] = z[79]*z[84] - z[81]*z[82];
    z[102] = z[76]*z[84] - z[78]*z[82];
    z[105] = (z[95]*u6+z[99]*u4-z[102]*u5)/z[97];
    q5p = -z[105];
    z[100] = z[79]*z[83] - z[80]*z[82];
    z[103] = z[76]*z[83] - z[77]*z[82];
    z[106] = (z[94]*u6+z[100]*u4-z[103]*u5)/z[97];
    q6p = z[106];
    z[39] = sin(q8);
    z[37] = sin(q9);
    z[52] = z[37]*z[39];
    z[34] = cos(q8);
    z[41] = sin(q7);
    z[44] = z[34]*z[41];
    z[55] = z[34]*z[37]*z[41];
    z[40] = cos(q7);
    z[47] = z[34]*z[40];
    z[56] = z[34]*z[37]*z[40];
    z[59] = z[39]*z[52] + z[44]*z[55] + z[47]*z[56];
    z[35] = cos(q9);
    z[36] = z[34]*z[35];
    z[45] = z[37]*z[41] - z[35]*z[39]*z[40];
    z[42] = z[37]*z[40] + z[35]*z[39]*z[41];
    z[67] = z[3]*z[36] + z[6]*z[45] - z[5]*z[42];
    z[57] = z[39]*z[41];
    z[58] = z[39]*z[40];
    z[62] = z[34]*z[36] + z[42]*z[57] - z[45]*z[58];
    z[46] = z[35]*z[41] + z[37]*z[39]*z[40];
    z[38] = z[34]*z[37];
    z[43] = z[35]*z[40] - z[37]*z[39]*z[41];
    z[70] = z[6]*z[46] - z[3]*z[38] - z[5]*z[43];
    z[48] = z[35]*z[39];
    z[50] = z[34]*z[35]*z[41];
    z[51] = z[34]*z[35]*z[40];
    z[64] = z[38]*z[48] + z[43]*z[50] - z[46]*z[51];
    z[73] = z[3]*z[39] + z[5]*z[44] + z[6]*z[47];
    z[86] = z[59]*z[67] + z[62]*z[70] + z[64]*z[73];
    z[49] = z[35]*z[39]*z[40] - z[37]*z[41];
    z[54] = -z[37]*z[40] - z[35]*z[39]*z[41];
    z[61] = z[47]*z[49] - z[36]*z[39] - z[44]*z[54];
    z[68] = z[9]*z[36] + z[10]*z[42] - z[11]*z[45];
    z[65] = z[38]**2 + z[43]**2 + z[46]**2;
    z[74] = z[9]*z[39] - z[10]*z[44] - z[11]*z[47];
    z[90] = z[61]*z[68] + z[65]*z[74];
    z[87] = z[61]*z[67] + z[65]*z[73];
    z[71] = z[10]*z[43] - z[9]*z[38] - z[11]*z[46];
    z[89] = z[59]*z[68] + z[62]*z[71] + z[64]*z[74];
    z[109] = z[86]*z[90] - z[87]*z[89];
    z[119] = u9 + z[83]*z[105] - z[82]*z[104] - z[84]*z[106];
    z[69] = z[12]*z[36] + z[13]*z[42] + z[14]*z[45];
    z[75] = z[12]*z[39] + z[14]*z[47] - z[13]*z[44];
    z[93] = z[61]*z[69] + z[65]*z[75];
    z[72] = z[13]*z[43] + z[14]*z[46] - z[12]*z[38];
    z[92] = z[59]*z[69] + z[62]*z[72] + z[64]*z[75];
    z[111] = z[89]*z[93] - z[90]*z[92];
    z[117] = u7 + z[77]*z[105] - z[76]*z[104] - z[78]*z[106];
    z[114] = z[86]*z[93] - z[87]*z[92];
    z[118] = u8 + z[80]*z[105] - z[79]*z[104] - z[81]*z[106];
    z[53] = -z[35]*z[41] - z[37]*z[39]*z[40];
    z[60] = z[43]*z[47] - z[44]*z[53];
    z[63] = -z[42]*z[47] - z[44]*z[45];
    z[66] = z[42]*z[46] + z[43]*z[49];
    z[91] = z[60]*z[69] + z[63]*z[72] + z[66]*z[75];
    z[85] = z[60]*z[67] + z[63]*z[70] + z[66]*z[73];
    z[88] = z[60]*z[68] + z[63]*z[71] + z[66]*z[74];
    z[107] = z[85]*z[89] - z[86]*z[88];
    z[108] = z[85]*z[90] - z[87]*z[88];
    z[110] = z[91]*z[109] + z[93]*z[107] - z[92]*z[108];
    z[120] = (z[109]*z[119]+z[111]*z[117]-z[114]*z[118])/z[110];
    q7p = z[120];
    z[112] = z[88]*z[93] - z[90]*z[91];
    z[115] = z[85]*z[93] - z[87]*z[91];
    z[121] = (z[108]*z[119]+z[112]*z[117]-z[115]*z[118])/z[110];
    q8p = -z[121];
    z[113] = z[88]*z[92] - z[89]*z[91];
    z[116] = z[85]*z[92] - z[86]*z[91];
    z[122] = (z[107]*z[119]+z[113]*z[117]-z[116]*z[118])/z[110];
    q9p = z[122];
    z[224] = z[145]*z[11];
    z[287] = m*z[224];
    z[236] = z[235]*z[46];
    z[241] = z[235]*z[44];
    z[237] = z[235]*z[47];
    z[240] = z[235]*z[43];
    z[363] = 0.0625*z[236]*z[241] + 0.0625*z[237]*z[240];
    z[226] = z[145]*z[71];
    z[227] = z[145]*z[74];
    z[385] = 0.125*z[226]*z[241] + 0.125*z[227]*z[240];
    z[380] = z[239]*z[224];
    z[260] = z[234]*z[240];
    z[225] = z[145]*z[10];
    z[382] = -0.125*z[224]*z[240] - 0.125*z[225]*z[236];
    z[360] = z[239]*z[241];
    z[381] = 0.125*z[225]*z[237] - 0.125*z[224]*z[241];
    z[267] = z[239]*z[237];
    z[383] = z[239]*z[227];
    z[361] = z[239]*z[236];
    z[384] = z[234]*z[226];
    z[386] = z[363]*z[385] - 0.5*z[257]*z[380] - 0.25*z[260]*z[382] - 0.25*z[360]*z[381] - 0.125*z[267]*z[383] - 0.125*z[361]*z[384];
    z[229] = z[145]*z[13];
    z[232] = z[145]*z[75];
    z[297] = 0.25*z[227]*z[229] - 0.25*z[225]*z[232];
    z[230] = z[145]*z[72];
    z[284] = 0.125*z[230]*z[237] - 0.125*z[232]*z[236];
    z[393] = 0.25*z[226]*z[232] - 0.25*z[227]*z[230];
    z[231] = z[145]*z[14];
    z[392] = 0.25*z[224]*z[229] + 0.25*z[225]*z[231];
    z[271] = z[234]*z[229];
    z[369] = 0.125*z[231]*z[240] - 0.125*z[229]*z[236];
    z[388] = 0.125*z[226]*z[237] - 0.125*z[227]*z[236];
    z[370] = z[239]*z[232];
    z[387] = z[234]*z[225];
    z[399] = z[284]*z[382] + 0.25*z[260]*z[393] + 0.25*z[267]*z[392] + 0.25*z[271]*z[383] - z[369]*z[388] - 0.25*z[370]*z[387];
    z[368] = 0.125*z[229]*z[237] + 0.125*z[231]*z[241];
    z[371] = z[234]*z[230];
    z[394] = z[257]*z[392] + z[363]*z[393] + 0.5*z[368]*z[383] + 0.5*z[371]*z[382] - 0.5*z[369]*z[384] - 0.5*z[370]*z[381];
    z[367] = z[239]*z[231];
    z[372] = 0.125*z[230]*z[241] + 0.125*z[232]*z[240];
    z[402] = z[257]*z[393] + z[363]*z[392] + 0.5*z[271]*z[385] - 0.5*z[284]*z[380] - 0.5*z[367]*z[388] - 0.5*z[372]*z[387];
    z[397] = 0.25*z[224]*z[230] + 0.25*z[226]*z[231];
    z[398] = z[372]*z[381] + 0.25*z[367]*z[384] + 0.25*z[371]*z[380] - z[368]*z[385] - 0.25*z[260]*z[392] - 0.25*z[267]*z[393];
    z[359] = z[234]*z[241];
    z[362] = z[234]*z[236];
    z[365] = z[364] + z[363]**2 - 0.0625*z[260]**2 - 0.0625*z[267]**2 - 0.0625*z[359]*z[360] - 0.0625*z[361]*z[362];
    z[302] = m*z[225];
    z[389] = z[363]*z[388] + 0.125*z[359]*z[383] + 0.25*z[362]*z[382] + 0.5*z[257]*z[387] - 0.25*z[267]*z[381] - 0.125*z[260]*z[384];
    z[309] = m*z[227];
    z[390] = z[363]*z[382] + 0.125*z[267]*z[380] + 0.125*z[360]*z[387] + 0.25*z[361]*z[388] + 0.5*z[257]*z[383] - 0.25*z[260]*z[385];
    z[312] = m*z[226];
    z[391] = z[363]*z[381] + 0.125*z[362]*z[380] + 0.5*z[257]*z[384] - 0.25*z[267]*z[388] - 0.25*z[359]*z[385] - 0.125*z[260]*z[387];
    z[366] = m*z[231];
    z[373] = z[363]*z[372] + 0.5*z[257]*z[367] - 0.25*z[260]*z[369] - 0.25*z[360]*z[368] - 0.125*z[267]*z[370] - 0.125*z[361]*z[371];
    z[374] = m*z[229];
    z[375] = z[284]*z[363] + 0.125*z[359]*z[370] + 0.25*z[362]*z[369] + 0.5*z[257]*z[271] - 0.25*z[267]*z[368] - 0.125*z[260]*z[371];
    z[376] = m*z[232];
    z[377] = z[363]*z[369] + 0.125*z[271]*z[360] + 0.25*z[284]*z[361] + 0.5*z[257]*z[370] - 0.25*z[260]*z[372] - 0.125*z[267]*z[367];
    z[378] = m*z[230];
    z[379] = z[363]*z[368] + 0.5*z[257]*z[371] - 0.25*z[267]*z[284] - 0.25*z[359]*z[372] - 0.125*z[260]*z[271] - 0.125*z[362]*z[367];
    z[395] = -0.25*z[224]*z[232] - 0.25*z[227]*z[231];
    z[396] = z[372]*z[382] + 0.25*z[360]*z[392] + 0.25*z[361]*z[393] + 0.25*z[367]*z[383] + 0.25*z[370]*z[380] - z[369]*z[385];
    z[400] = 0.25*z[225]*z[230] - 0.25*z[226]*z[229];
    z[401] = z[284]*z[381] + 0.25*z[271]*z[384] + 0.25*z[359]*z[393] + 0.25*z[362]*z[392] - z[368]*z[388] - 0.25*z[371]*z[387];
    z[403] = z[287]*z[386] + z[297]*z[399] + z[392]*z[394] + z[393]*z[402] + z[397]*z[398] + 4*z[254]*z[365] - z[302]*z[389] - z[309]*z[390] - z[312]*z[391] - z[366]*z[373] - z[374]*z[375] - z[376]*z[377] - z[378]*z[379] - z[395]*z[396] - z[400]*z[401];
    z[124] = L*u5;
    z[128] = u4*z[124];
    z[123] = L*u6;
    z[127] = 0.5*u5*z[124] + 0.5*u6*z[123];
    z[126] = L*u8;
    z[125] = L*u9;
    z[130] = 0.5*u8*z[126] + 0.5*u9*z[125];
    z[129] = u4*z[123];
    z[131] = u7*z[126];
    z[132] = u7*z[125];
    z[223] = m*(z[5]*z[128]+2*z[3]*z[127]+2*z[67]*z[130]-z[6]*z[129]-z[70]*z[131]-z[73]*z[132]);
    z[221] = z[145]*z[6];
    z[314] = m*z[221];
    z[222] = z[145]*z[73];
    z[336] = m*z[222];
    z[220] = z[145]*z[70];
    z[339] = m*z[220];
    z[219] = z[145]*z[5];
    z[341] = 0.25*z[219]*z[224] - 0.25*z[221]*z[225];
    z[342] = 0.25*z[219]*z[227] + 0.25*z[222]*z[225];
    z[350] = -0.25*z[220]*z[224] - 0.25*z[221]*z[226];
    z[356] = 0.25*z[220]*z[227] - 0.25*z[222]*z[226];
    z[329] = m*z[219];
    z[347] = 0.25*z[221]*z[227] + 0.25*z[222]*z[224];
    z[354] = -0.25*z[219]*z[226] - 0.25*z[220]*z[225];
    z[450] = z[314]*z[373] + z[336]*z[377] + z[339]*z[379] + z[341]*z[394] + z[342]*z[399] + z[350]*z[398] + z[356]*z[402] - z[329]*z[375] - z[347]*z[396] - z[354]*z[401];
    z[233] = m*(2*z[12]*z[127]+2*z[69]*z[130]-z[13]*z[128]-z[14]*z[129]-z[72]*z[131]-z[75]*z[132]);
    z[324] = 0.25*z[219]*z[232] + 0.25*z[222]*z[229];
    z[416] = -0.25*z[219]*z[231] - 0.25*z[221]*z[229];
    z[441] = 0.25*z[220]*z[231] - 0.25*z[221]*z[230];
    z[417] = 0.25*z[220]*z[232] - 0.25*z[222]*z[230];
    z[440] = 0.25*z[221]*z[232] - 0.25*z[222]*z[231];
    z[442] = -0.25*z[219]*z[230] - 0.25*z[220]*z[229];
    z[443] = z[324]*z[399] + z[329]*z[389] + z[394]*z[416] + z[398]*z[441] + z[402]*z[417] - z[314]*z[386] - z[336]*z[390] - z[339]*z[391] - z[396]*z[440] - z[401]*z[442];
    z[228] = m*(z[11]*z[129]+2*z[9]*z[127]+2*z[68]*z[130]-z[10]*z[128]-z[71]*z[131]-z[74]*z[132]);
    z[245] = -2*z[206] - 0.5*z[228];
    z[458] = 2*z[457] - 0.25*z[229]**2;
    z[276] = -0.5*z[239]*z[232] - 0.125*z[229]*z[241];
    z[461] = 0.5*z[239]*z[230] - 0.125*z[229]*z[240];
    z[460] = 0.5*m*z[240] - 0.25*z[229]*z[230];
    z[459] = 0.25*z[229]*z[232] + 0.5*m*z[241];
    z[462] = z[257]*z[458] + z[372]**2 + 0.5*z[276]*z[370] - 0.5*z[371]*z[461] - 0.25*z[260]*z[460] - 0.25*z[360]*z[459];
    z[262] = z[229]*z[231];
    z[464] = z[231]*z[240];
    z[466] = z[284]*z[461] + z[363]*z[460] + 0.0625*z[262]*z[360] + 0.25*z[367]*z[370] - 0.25*z[267]*z[458] - 0.125*z[372]*z[464];
    z[463] = z[231]*z[241];
    z[467] = z[276]*z[284] + z[363]*z[459] + 0.25*z[367]*z[371] - 0.25*z[362]*z[458] - 0.125*z[372]*z[463] - 0.0625*z[260]*z[262];
    z[468] = z[225]*z[232];
    z[469] = z[225]*z[230];
    z[470] = z[257]*z[302] + z[372]*z[393] + 0.5*z[383]*z[459] - 0.5*z[384]*z[460] - 0.125*z[370]*z[468] - 0.125*z[371]*z[469];
    z[474] = z[225]*z[231];
    z[475] = 0.125*z[262]*z[383] + 0.125*z[393]*z[464] + 0.25*z[267]*z[302] - z[388]*z[460] - 0.25*z[284]*z[469] - 0.125*z[370]*z[474];
    z[471] = z[225]*z[229];
    z[473] = z[276]*z[393] + 0.125*z[371]*z[471] + 0.25*z[372]*z[468] + 0.5*z[384]*z[458] - z[385]*z[459] - 0.25*z[260]*z[302];
    z[477] = z[302]*z[363] + 0.25*z[262]*z[385] + 0.5*z[367]*z[393] - z[388]*z[458] - 0.25*z[284]*z[471] - 0.25*z[372]*z[474];
    z[465] = z[284]*z[372] + 0.0625*z[370]*z[463] + 0.25*z[257]*z[262] + 0.25*z[362]*z[460] - 0.25*z[267]*z[459] - 0.0625*z[371]*z[464];
    z[472] = z[393]*z[461] + 0.125*z[370]*z[471] + 0.25*z[302]*z[360] + 0.5*z[383]*z[458] - z[385]*z[460] - 0.25*z[372]*z[469];
    z[476] = 0.125*z[262]*z[384] + 0.125*z[393]*z[463] + 0.25*z[284]*z[468] + 0.25*z[302]*z[362] - z[388]*z[459] - 0.125*z[371]*z[474];
    z[478] = z[314]*z[462] + z[336]*z[466] + z[339]*z[467] + z[341]*z[470] + z[342]*z[475] + z[350]*z[473] + z[356]*z[477] - z[329]*z[465] - z[347]*z[472] - z[354]*z[476];
    z[208] = 0.5*z[207]*z[11] - T2;
    z[133] = I1*u4;
    z[135] = I3*u6;
    z[214] = u6*z[133] - u4*z[135];
    z[238] = z[214] + 0.25*z[145]*(2*z[45]*z[130]-z[129]-z[46]*z[131]-z[47]*z[132]);
    z[247] = z[208] - z[238];
    z[266] = 0.5*m*z[237] - 0.25*z[231]*z[232];
    z[557] = z[556] - 0.0625*z[240]**2;
    z[280] = z[229]*z[237];
    z[558] = 0.125*z[230]*z[240] - 0.5*z[239]*z[229];
    z[259] = z[240]*z[241];
    z[269] = 0.25*z[230]*z[231] - 0.5*m*z[236];
    z[282] = z[229]*z[236];
    z[559] = z[230]*z[241];
    z[560] = z[284]*z[461] + 0.0625*z[262]*z[360] - z[266]*z[557] - 0.125*z[280]*z[558] - 0.0625*z[259]*z[269] - 0.015625*z[282]*z[559];
    z[256] = 2*z[255] - 0.25*z[231]**2;
    z[278] = 0.5*z[234]*z[230] - 0.125*z[231]*z[236];
    z[561] = z[236]*z[240];
    z[564] = z[256]*z[557] + 0.015625*z[282]**2 + 0.25*z[269]*z[361] + 0.5*z[271]*z[558] - z[278]*z[461] - 0.015625*z[262]*z[561];
    z[275] = 0.125*z[231]*z[237] - 0.5*z[234]*z[232];
    z[562] = z[236]*z[241];
    z[565] = 0.0625*z[271]*z[559] + 0.25*z[266]*z[361] - z[275]*z[461] - 0.0625*z[256]*z[259] - 0.015625*z[262]*z[562] - 0.015625*z[280]*z[282];
    z[566] = 0.125*z[226]*z[240] - 0.5*z[239]*z[225];
    z[289] = z[224]*z[232];
    z[567] = z[226]*z[241];
    z[290] = z[224]*z[230];
    z[568] = z[266]*z[566] + z[284]*z[400] + 0.25*z[287]*z[360] - 0.25*z[289]*z[558] - 0.125*z[269]*z[567] - 0.03125*z[290]*z[559];
    z[303] = z[224]*z[231];
    z[572] = z[225]*z[236];
    z[573] = z[256]*z[566] + z[278]*z[400] + 0.0625*z[287]*z[561] - 0.25*z[303]*z[558] - 0.125*z[269]*z[572] - 0.03125*z[282]*z[290];
    z[288] = z[224]*z[229];
    z[569] = 0.5*z[239]*z[226] - 0.125*z[225]*z[240];
    z[571] = 0.03125*z[262]*z[567] + 0.03125*z[288]*z[559] + 0.25*z[289]*z[461] - z[266]*z[569] - 0.125*z[280]*z[400] - 0.0625*z[259]*z[287];
    z[575] = z[256]*z[569] + 0.25*z[287]*z[361] + 0.5*z[271]*z[400] - 0.25*z[303]*z[461] - 0.03125*z[262]*z[572] - 0.03125*z[282]*z[288];
    z[563] = z[275]*z[558] + 0.0625*z[269]*z[562] + 0.125*z[282]*z[284] + 0.25*z[256]*z[360] - 0.125*z[278]*z[559] - 0.0625*z[266]*z[561];
    z[570] = z[287]*z[557] + 0.125*z[282]*z[400] + 0.25*z[262]*z[566] + 0.25*z[288]*z[558] - z[269]*z[569] - 0.25*z[290]*z[461];
    z[574] = z[275]*z[400] + 0.03125*z[282]*z[289] + 0.0625*z[287]*z[562] + 0.125*z[256]*z[567] - 0.125*z[266]*z[572] - 0.03125*z[303]*z[559];
    z[576] = z[314]*z[560] + z[336]*z[564] + z[339]*z[565] + z[341]*z[568] + z[342]*z[573] + z[350]*z[571] + z[356]*z[575] - z[329]*z[563] - z[347]*z[570] - z[354]*z[574];
    z[211] = T2*z[43] + T3*z[46] - T1*z[38] - 0.5*z[207]*z[74];
    z[136] = I1*u7;
    z[138] = I3*u9;
    z[217] = u9*z[136] - u7*z[138];
    z[243] = z[217] + 0.25*z[145]*(z[44]*z[128]+2*z[39]*z[127]-z[132]-z[47]*z[129]);
    z[250] = z[211] - z[243];
    z[514] = z[284]*z[372] + 0.25*z[257]*z[262] - 0.25*z[260]*z[269] - 0.25*z[266]*z[360] - 0.0625*z[280]*z[370] - 0.0625*z[282]*z[371];
    z[516] = z[269]*z[363] + 0.125*z[282]*z[284] + 0.25*z[256]*z[360] + 0.25*z[271]*z[370] - z[278]*z[372] - 0.0625*z[262]*z[267];
    z[517] = z[266]*z[363] + 0.25*z[271]*z[371] - z[275]*z[372] - 0.25*z[256]*z[260] - 0.125*z[280]*z[284] - 0.0625*z[262]*z[362];
    z[518] = z[257]*z[287] + z[284]*z[393] + 0.5*z[266]*z[383] - 0.5*z[269]*z[384] - 0.125*z[289]*z[370] - 0.125*z[290]*z[371];
    z[521] = z[278]*z[393] + 0.25*z[267]*z[287] + 0.5*z[256]*z[383] - z[269]*z[388] - 0.25*z[284]*z[290] - 0.125*z[303]*z[370];
    z[520] = 0.125*z[262]*z[384] + 0.125*z[288]*z[371] + 0.25*z[289]*z[372] - z[266]*z[385] - 0.25*z[260]*z[287] - 0.125*z[280]*z[393];
    z[523] = z[256]*z[385] + z[287]*z[363] + 0.5*z[271]*z[393] - 0.25*z[262]*z[388] - 0.25*z[284]*z[288] - 0.25*z[303]*z[372];
    z[515] = z[257]*z[256] + z[284]**2 + 0.25*z[269]*z[362] + 0.5*z[275]*z[370] - 0.5*z[278]*z[371] - 0.25*z[266]*z[267];
    z[519] = 0.125*z[262]*z[383] + 0.125*z[282]*z[393] + 0.125*z[288]*z[370] + 0.25*z[287]*z[360] - z[269]*z[385] - 0.25*z[290]*z[372];
    z[522] = z[275]*z[393] + 0.25*z[284]*z[289] + 0.25*z[287]*z[362] + 0.5*z[256]*z[384] - z[266]*z[388] - 0.125*z[303]*z[371];
    z[524] = z[314]*z[514] + z[336]*z[516] + z[339]*z[517] + z[341]*z[518] + z[342]*z[521] + z[350]*z[520] + z[356]*z[523] - z[329]*z[515] - z[347]*z[519] - z[354]*z[522];
    z[209] = 0.5*z[207]*z[10] - T3;
    z[134] = I2*u5;
    z[213] = u4*z[134] - u5*z[133];
    z[242] = z[213] + 0.25*z[145]*(z[128]+z[43]*z[131]-2*z[42]*z[130]-z[44]*z[132]);
    z[248] = z[209] - z[242];
    z[258] = z[257] - 0.0625*z[241]**2;
    z[273] = z[232]*z[240];
    z[272] = -0.5*z[234]*z[229] - 0.125*z[232]*z[241];
    z[612] = z[258]*z[269] + z[276]*z[284] + 0.015625*z[273]*z[280] + 0.0625*z[259]*z[266] - 0.125*z[272]*z[282] - 0.0625*z[260]*z[262];
    z[264] = z[237]*z[240];
    z[614] = 0.015625*z[262]*z[264] - z[276]*z[278] - 0.25*z[267]*z[269] - 0.0625*z[256]*z[259] - 0.0625*z[271]*z[273] - 0.015625*z[280]*z[282];
    z[263] = z[237]*z[241];
    z[615] = z[256]*z[258] + 0.015625*z[262]*z[263] + 0.015625*z[280]**2 + 0.5*z[271]*z[272] - z[275]*z[276] - 0.25*z[266]*z[267];
    z[291] = -0.5*z[234]*z[225] - 0.125*z[227]*z[241];
    z[292] = z[227]*z[240];
    z[616] = z[284]*z[297] + 0.03125*z[273]*z[289] - z[269]*z[291] - 0.25*z[260]*z[287] - 0.25*z[272]*z[290] - 0.125*z[266]*z[292];
    z[304] = z[225]*z[237];
    z[619] = z[278]*z[297] + 0.03125*z[273]*z[303] + 0.03125*z[280]*z[290] + 0.125*z[269]*z[304] - 0.125*z[256]*z[292] - 0.0625*z[264]*z[287];
    z[294] = -0.5*z[239]*z[227] - 0.125*z[225]*z[241];
    z[618] = z[258]*z[287] + 0.25*z[262]*z[291] + 0.25*z[272]*z[288] + 0.25*z[276]*z[289] - z[266]*z[294] - 0.125*z[280]*z[297];
    z[621] = z[256]*z[294] + 0.03125*z[262]*z[304] + 0.03125*z[280]*z[288] + 0.5*z[271]*z[297] - 0.25*z[267]*z[287] - 0.25*z[276]*z[303];
    z[613] = 0.0625*z[264]*z[266] - z[272]*z[278] - 0.25*z[256]*z[260] - 0.125*z[273]*z[275] - 0.125*z[280]*z[284] - 0.0625*z[263]*z[269];
    z[617] = 0.125*z[282]*z[297] - z[269]*z[294] - 0.25*z[276]*z[290] - 0.0625*z[259]*z[287] - 0.03125*z[262]*z[292] - 0.03125*z[273]*z[288];
    z[620] = z[256]*z[291] + z[275]*z[297] + 0.125*z[266]*z[304] - 0.25*z[272]*z[303] - 0.0625*z[263]*z[287] - 0.03125*z[280]*z[289];
    z[622] = z[314]*z[612] + z[336]*z[614] + z[339]*z[615] + z[341]*z[616] + z[342]*z[619] + z[350]*z[618] + z[356]*z[621] - z[329]*z[613] - z[347]*z[617] - z[354]*z[620];
    z[212] = T1*z[39] + T3*z[47] + 0.5*z[207]*z[71] - T2*z[44];
    z[137] = I2*u8;
    z[216] = u7*z[137] - u8*z[136];
    z[244] = z[216] + 0.25*z[145]*(z[131]+z[43]*z[128]+z[46]*z[129]+2*z[38]*z[127]);
    z[251] = z[212] - z[244];
    z[293] = -z[239]*z[291] - 0.125*z[226]*z[260];
    z[265] = 0.0625*z[239]*z[263] - 0.0625*z[236]*z[260];
    z[306] = -0.25*z[236]*z[291] - 0.03125*z[226]*z[263];
    z[298] = 0.0625*z[230]*z[292] - z[239]*z[297] - 0.0625*z[226]*z[273];
    z[307] = 0.0625*z[230]*z[304] + 0.25*z[236]*z[297] - 0.0625*z[226]*z[280];
    z[281] = 0.03125*z[236]*z[273] + 0.125*z[239]*z[280] - 0.03125*z[230]*z[264];
    z[283] = -0.25*z[236]*z[272] - 0.03125*z[230]*z[263];
    z[305] = 0.03125*z[236]*z[292] + 0.125*z[239]*z[304] - 0.03125*z[226]*z[264];
    z[299] = 0.5*z[226]*z[272] - 0.5*z[230]*z[291];
    z[274] = -z[239]*z[272] - 0.125*z[230]*z[260];
    z[308] = z[256]*z[293] + z[265]*z[287] + z[269]*z[306] + z[275]*z[298] + z[284]*z[307] + 0.25*z[281]*z[289] + 0.25*z[283]*z[290] - z[266]*z[305] - z[278]*z[299] - 0.25*z[274]*z[303];
    z[295] = -z[239]*z[294] - 0.03125*z[226]*z[259] - 0.03125*z[240]*z[292];
    z[268] = 0.25*z[239]*z[267] - 0.015625*z[236]*z[259] - 0.015625*z[240]*z[264];
    z[310] = 0.03125*z[240]*z[304] - 0.25*z[236]*z[294] - 0.125*z[226]*z[267];
    z[285] = 0.03125*z[240]*z[280] - 0.25*z[236]*z[276] - 0.125*z[230]*z[267];
    z[300] = 0.5*z[226]*z[276] - 0.5*z[230]*z[294] - 0.25*z[240]*z[297];
    z[277] = -z[239]*z[276] - 0.03125*z[230]*z[259] - 0.03125*z[240]*z[273];
    z[311] = z[256]*z[295] + z[268]*z[287] + z[269]*z[310] + 0.125*z[282]*z[307] + 0.25*z[285]*z[290] + 0.5*z[271]*z[298] - z[278]*z[300] - 0.25*z[262]*z[305] - 0.25*z[277]*z[303] - 0.25*z[281]*z[288];
    z[296] = 0.25*z[240]*z[291] + 0.5*z[226]*z[258];
    z[270] = 0.25*z[236]*z[258] - 0.015625*z[240]*z[263];
    z[279] = 0.25*z[240]*z[272] + 0.5*z[230]*z[258];
    z[313] = z[256]*z[296] + z[266]*z[310] + z[270]*z[287] + 0.5*z[271]*z[299] - z[275]*z[300] - 0.25*z[262]*z[306] - 0.25*z[279]*z[303] - 0.25*z[283]*z[288] - 0.25*z[285]*z[289] - 0.125*z[280]*z[307];
    z[318] = 0.5*z[234]*z[219] - 0.125*z[222]*z[241];
    z[323] = 0.25*z[240]*z[318] + 0.5*z[220]*z[258];
    z[321] = 0.125*z[219]*z[241] - 0.5*z[239]*z[222];
    z[327] = 0.5*z[220]*z[276] - 0.5*z[230]*z[321] - 0.25*z[240]*z[324];
    z[320] = -z[239]*z[318] - 0.125*z[220]*z[260];
    z[261] = 0.0625*z[240]*z[260] - z[239]*z[258];
    z[319] = z[222]*z[240];
    z[322] = -z[239]*z[321] - 0.03125*z[220]*z[259] - 0.03125*z[240]*z[319];
    z[315] = z[221]*z[229];
    z[316] = z[221]*z[232];
    z[317] = z[221]*z[230];
    z[325] = 0.0625*z[230]*z[319] - z[239]*z[324] - 0.0625*z[220]*z[273];
    z[326] = 0.5*z[220]*z[272] - 0.5*z[230]*z[318];
    z[328] = z[269]*z[323] + z[284]*z[327] + 0.25*z[262]*z[320] - z[261]*z[314] - z[266]*z[322] - 0.25*z[274]*z[315] - 0.25*z[277]*z[316] - 0.25*z[279]*z[317] - 0.125*z[280]*z[325] - 0.125*z[282]*z[326];
    z[331] = z[219]*z[237];
    z[337] = -0.25*z[236]*z[321] - 0.125*z[220]*z[267] - 0.03125*z[240]*z[331];
    z[334] = 0.25*z[236]*z[324] - 0.0625*z[220]*z[280] - 0.0625*z[230]*z[331];
    z[330] = z[221]*z[231];
    z[332] = 0.03125*z[236]*z[319] - 0.125*z[239]*z[331] - 0.03125*z[220]*z[264];
    z[338] = z[256]*z[322] + z[269]*z[337] + 0.125*z[282]*z[334] + 0.25*z[277]*z[330] + 0.25*z[281]*z[315] + 0.5*z[271]*z[325] - z[268]*z[314] - z[278]*z[327] - 0.25*z[262]*z[332] - 0.25*z[285]*z[317];
    z[333] = -0.25*z[236]*z[318] - 0.03125*z[220]*z[263];
    z[340] = z[256]*z[323] + z[266]*z[337] + 0.25*z[279]*z[330] + 0.25*z[283]*z[315] + 0.25*z[285]*z[316] + 0.5*z[271]*z[326] - z[270]*z[314] - z[275]*z[327] - 0.25*z[262]*z[333] - 0.125*z[280]*z[334];
    z[343] = 0.0625*z[226]*z[319] - z[239]*z[342] - 0.0625*z[220]*z[292];
    z[345] = 0.5*z[220]*z[297] + 0.5*z[230]*z[342] - 0.5*z[226]*z[324];
    z[344] = 0.5*z[220]*z[291] - 0.5*z[226]*z[318];
    z[346] = z[266]*z[343] + z[284]*z[345] + z[287]*z[320] + z[293]*z[314] - z[269]*z[344] - 0.25*z[289]*z[325] - 0.25*z[290]*z[326] - 0.25*z[298]*z[316] - 0.25*z[299]*z[317];
    z[352] = 0.25*z[236]*z[342] - 0.0625*z[220]*z[304] - 0.0625*z[226]*z[331];
    z[353] = z[256]*z[343] + z[278]*z[345] + z[287]*z[332] + z[305]*z[314] - z[269]*z[352] - 0.25*z[290]*z[334] - 0.25*z[298]*z[330] - 0.25*z[303]*z[325] - 0.25*z[307]*z[317];
    z[348] = 0.5*z[220]*z[294] - 0.5*z[226]*z[321] - 0.25*z[240]*z[342];
    z[351] = z[287]*z[323] + z[296]*z[314] + 0.25*z[262]*z[344] + 0.25*z[288]*z[326] + 0.25*z[289]*z[327] + 0.25*z[299]*z[315] + 0.25*z[300]*z[316] - z[266]*z[348] - 0.125*z[280]*z[345];
    z[357] = z[256]*z[348] + z[287]*z[337] + z[310]*z[314] + 0.5*z[271]*z[345] - 0.25*z[262]*z[352] - 0.25*z[288]*z[334] - 0.25*z[300]*z[330] - 0.25*z[303]*z[327] - 0.25*z[307]*z[315];
    z[286] = z[256]*z[261] + z[266]*z[268] + z[278]*z[279] + z[284]*z[285] + 0.5*z[271]*z[274] - z[269]*z[270] - z[275]*z[277] - 0.25*z[262]*z[265] - 0.125*z[280]*z[281] - 0.125*z[282]*z[283];
    z[301] = z[261]*z[287] + z[269]*z[296] + z[284]*z[300] + 0.25*z[262]*z[293] + 0.25*z[274]*z[288] + 0.25*z[277]*z[289] + 0.25*z[279]*z[290] - z[266]*z[295] - 0.125*z[280]*z[298] - 0.125*z[282]*z[299];
    z[335] = z[256]*z[320] + z[269]*z[333] + z[275]*z[325] + z[284]*z[334] + 0.25*z[274]*z[330] - z[265]*z[314] - z[266]*z[332] - z[278]*z[326] - 0.25*z[281]*z[316] - 0.25*z[283]*z[317];
    z[349] = z[287]*z[322] + z[295]*z[314] + 0.125*z[282]*z[345] + 0.25*z[262]*z[343] + 0.25*z[288]*z[325] + 0.25*z[298]*z[315] - z[269]*z[348] - 0.25*z[290]*z[327] - 0.25*z[300]*z[317];
    z[355] = z[256]*z[344] + z[275]*z[345] + z[287]*z[333] + z[306]*z[314] + 0.25*z[289]*z[334] + 0.25*z[307]*z[316] - z[266]*z[352] - 0.25*z[299]*z[330] - 0.25*z[303]*z[326];
    z[358] = z[302]*z[308] + z[309]*z[311] + z[312]*z[313] + z[314]*z[328] + z[336]*z[338] + z[339]*z[340] + z[341]*z[346] + z[342]*z[353] + z[350]*z[351] + z[356]*z[357] + 4*z[254]*z[286] - z[287]*z[301] - z[329]*z[335] - z[347]*z[349] - z[354]*z[355];
    z[654] = (z[403]*z[223]+z[450]*z[233]+2*z[443]*z[245]+2*z[478]*z[247]+2*z[576]*z[250]-2*z[524]*z[248]-2*z[622]*z[251])/z[358];
    u1p = 0.5*z[654];
    z[409] = 0.125*z[220]*z[241] + 0.125*z[222]*z[240];
    z[404] = z[239]*z[221];
    z[406] = 0.125*z[219]*z[236] + 0.125*z[221]*z[240];
    z[405] = 0.125*z[221]*z[241] - 0.125*z[219]*z[237];
    z[407] = z[239]*z[222];
    z[408] = z[234]*z[220];
    z[410] = z[363]*z[409] + 0.5*z[257]*z[404] - 0.25*z[260]*z[406] - 0.25*z[360]*z[405] - 0.125*z[267]*z[407] - 0.125*z[361]*z[408];
    z[411] = z[234]*z[219];
    z[412] = 0.125*z[220]*z[237] - 0.125*z[222]*z[236];
    z[421] = z[284]*z[406] + 0.25*z[260]*z[417] + 0.25*z[267]*z[416] + 0.25*z[271]*z[407] + 0.25*z[370]*z[411] - z[369]*z[412];
    z[418] = z[257]*z[416] + z[363]*z[417] + 0.5*z[368]*z[407] + 0.5*z[371]*z[406] - 0.5*z[369]*z[408] - 0.5*z[370]*z[405];
    z[423] = z[257]*z[417] + z[363]*z[416] + 0.5*z[271]*z[409] + 0.5*z[284]*z[404] + 0.5*z[372]*z[411] - 0.5*z[367]*z[412];
    z[420] = z[372]*z[405] + 0.25*z[367]*z[408] - z[368]*z[409] - 0.25*z[260]*z[416] - 0.25*z[267]*z[417] - 0.25*z[371]*z[404];
    z[413] = z[363]*z[412] + 0.125*z[359]*z[407] + 0.25*z[362]*z[406] - 0.5*z[257]*z[411] - 0.25*z[267]*z[405] - 0.125*z[260]*z[408];
    z[414] = z[363]*z[406] + 0.25*z[361]*z[412] + 0.5*z[257]*z[407] - 0.25*z[260]*z[409] - 0.125*z[267]*z[404] - 0.125*z[360]*z[411];
    z[415] = z[363]*z[405] + 0.125*z[260]*z[411] + 0.5*z[257]*z[408] - 0.25*z[267]*z[412] - 0.25*z[359]*z[409] - 0.125*z[362]*z[404];
    z[419] = z[372]*z[406] + 0.25*z[360]*z[416] + 0.25*z[361]*z[417] + 0.25*z[367]*z[407] - z[369]*z[409] - 0.25*z[370]*z[404];
    z[422] = z[284]*z[405] + 0.25*z[271]*z[408] + 0.25*z[359]*z[417] + 0.25*z[362]*z[416] + 0.25*z[371]*z[411] - z[368]*z[412];
    z[424] = z[287]*z[410] + z[297]*z[421] + z[392]*z[418] + z[393]*z[423] + z[397]*z[420] - z[302]*z[413] - z[309]*z[414] - z[312]*z[415] - z[395]*z[419] - z[400]*z[422];
    z[451] = z[287]*z[373] + z[341]*z[418] + z[342]*z[421] + z[350]*z[420] + z[356]*z[423] - z[302]*z[375] - z[309]*z[377] - z[312]*z[379] - z[347]*z[419] - z[354]*z[422];
    z[444] = z[324]*z[421] + z[329]*z[413] + z[416]*z[418] + z[417]*z[423] + z[420]*z[441] + 4*z[254]*z[365] - z[314]*z[410] - z[336]*z[414] - z[339]*z[415] - z[366]*z[373] - z[374]*z[375] - z[376]*z[377] - z[378]*z[379] - z[419]*z[440] - z[422]*z[442];
    z[479] = z[219]*z[232];
    z[480] = z[219]*z[230];
    z[481] = z[372]*z[417] + 0.125*z[370]*z[479] + 0.125*z[371]*z[480] + 0.5*z[407]*z[459] - z[257]*z[329] - 0.5*z[408]*z[460];
    z[485] = z[219]*z[231];
    z[486] = 0.125*z[262]*z[407] + 0.125*z[370]*z[485] + 0.125*z[417]*z[464] + 0.25*z[284]*z[480] - z[412]*z[460] - 0.25*z[267]*z[329];
    z[482] = z[219]*z[229];
    z[484] = z[276]*z[417] + 0.25*z[260]*z[329] + 0.5*z[408]*z[458] - z[409]*z[459] - 0.25*z[372]*z[479] - 0.125*z[371]*z[482];
    z[488] = 0.25*z[262]*z[409] + 0.25*z[284]*z[482] + 0.25*z[372]*z[485] + 0.5*z[367]*z[417] - z[329]*z[363] - z[412]*z[458];
    z[483] = z[417]*z[461] + 0.25*z[372]*z[480] + 0.5*z[407]*z[458] - z[409]*z[460] - 0.25*z[329]*z[360] - 0.125*z[370]*z[482];
    z[487] = 0.125*z[262]*z[408] + 0.125*z[371]*z[485] + 0.125*z[417]*z[463] - z[412]*z[459] - 0.25*z[284]*z[479] - 0.25*z[329]*z[362];
    z[489] = z[287]*z[462] + z[341]*z[481] + z[342]*z[486] + z[350]*z[484] + z[356]*z[488] - z[302]*z[465] - z[309]*z[466] - z[312]*z[467] - z[347]*z[483] - z[354]*z[487];
    z[577] = 0.125*z[220]*z[240] + 0.5*z[239]*z[219];
    z[578] = z[220]*z[241];
    z[579] = z[266]*z[577] + z[284]*z[442] + 0.03125*z[317]*z[559] + 0.25*z[316]*z[558] - 0.25*z[314]*z[360] - 0.125*z[269]*z[578];
    z[583] = z[219]*z[236];
    z[584] = z[256]*z[577] + z[278]*z[442] + 0.03125*z[282]*z[317] + 0.125*z[269]*z[583] + 0.25*z[330]*z[558] - 0.0625*z[314]*z[561];
    z[580] = 0.125*z[219]*z[240] + 0.5*z[239]*z[220];
    z[582] = 0.03125*z[262]*z[578] + 0.0625*z[259]*z[314] - z[266]*z[580] - 0.25*z[316]*z[461] - 0.125*z[280]*z[442] - 0.03125*z[315]*z[559];
    z[586] = z[256]*z[580] + 0.03125*z[262]*z[583] + 0.03125*z[282]*z[315] + 0.25*z[330]*z[461] + 0.5*z[271]*z[442] - 0.25*z[314]*z[361];
    z[581] = 0.125*z[282]*z[442] + 0.25*z[262]*z[577] + 0.25*z[317]*z[461] - z[269]*z[580] - z[314]*z[557] - 0.25*z[315]*z[558];
    z[585] = z[275]*z[442] + 0.03125*z[330]*z[559] + 0.125*z[256]*z[578] + 0.125*z[266]*z[583] - 0.0625*z[314]*z[562] - 0.03125*z[282]*z[316];
    z[587] = z[287]*z[560] + z[341]*z[579] + z[342]*z[584] + z[350]*z[582] + z[356]*z[586] - z[302]*z[563] - z[309]*z[564] - z[312]*z[565] - z[347]*z[581] - z[354]*z[585];
    z[525] = z[284]*z[417] + 0.125*z[316]*z[370] + 0.125*z[317]*z[371] + 0.5*z[266]*z[407] - z[257]*z[314] - 0.5*z[269]*z[408];
    z[528] = z[278]*z[417] + 0.125*z[330]*z[370] + 0.25*z[284]*z[317] + 0.5*z[256]*z[407] - z[269]*z[412] - 0.25*z[267]*z[314];
    z[527] = 0.125*z[262]*z[408] + 0.25*z[260]*z[314] - z[266]*z[409] - 0.25*z[316]*z[372] - 0.125*z[280]*z[417] - 0.125*z[315]*z[371];
    z[530] = z[256]*z[409] + 0.25*z[284]*z[315] + 0.25*z[330]*z[372] + 0.5*z[271]*z[417] - z[314]*z[363] - 0.25*z[262]*z[412];
    z[526] = 0.125*z[262]*z[407] + 0.125*z[282]*z[417] + 0.25*z[317]*z[372] - z[269]*z[409] - 0.25*z[314]*z[360] - 0.125*z[315]*z[370];
    z[529] = z[275]*z[417] + 0.125*z[330]*z[371] + 0.5*z[256]*z[408] - z[266]*z[412] - 0.25*z[284]*z[316] - 0.25*z[314]*z[362];
    z[531] = z[287]*z[514] + z[341]*z[525] + z[342]*z[528] + z[350]*z[527] + z[356]*z[530] - z[302]*z[515] - z[309]*z[516] - z[312]*z[517] - z[347]*z[526] - z[354]*z[529];
    z[623] = z[284]*z[324] + 0.25*z[260]*z[314] + 0.25*z[272]*z[317] - z[269]*z[318] - 0.125*z[266]*z[319] - 0.03125*z[273]*z[316];
    z[626] = z[278]*z[324] + 0.0625*z[264]*z[314] - 0.125*z[256]*z[319] - 0.125*z[269]*z[331] - 0.03125*z[273]*z[330] - 0.03125*z[280]*z[317];
    z[625] = 0.25*z[262]*z[318] - z[258]*z[314] - z[266]*z[321] - 0.25*z[272]*z[315] - 0.25*z[276]*z[316] - 0.125*z[280]*z[324];
    z[628] = z[256]*z[321] + 0.25*z[267]*z[314] + 0.25*z[276]*z[330] + 0.5*z[271]*z[324] - 0.03125*z[262]*z[331] - 0.03125*z[280]*z[315];
    z[624] = 0.03125*z[273]*z[315] + 0.0625*z[259]*z[314] + 0.125*z[282]*z[324] + 0.25*z[276]*z[317] - z[269]*z[321] - 0.03125*z[262]*z[319];
    z[627] = z[256]*z[318] + z[275]*z[324] + 0.03125*z[280]*z[316] + 0.0625*z[263]*z[314] + 0.25*z[272]*z[330] - 0.125*z[266]*z[331];
    z[629] = z[287]*z[612] + z[341]*z[623] + z[342]*z[626] + z[350]*z[625] + z[356]*z[628] - z[302]*z[613] - z[309]*z[614] - z[312]*z[615] - z[347]*z[624] - z[354]*z[627];
    z[655] = (z[424]*z[223]+z[451]*z[233]+2*z[444]*z[245]+2*z[489]*z[247]+2*z[587]*z[250]-2*z[531]*z[248]-2*z[629]*z[251])/z[358];
    u2p = -0.5*z[655];
    z[428] = z[388]*z[406] + 0.25*z[260]*z[356] + 0.25*z[267]*z[341] + 0.25*z[383]*z[411] + 0.25*z[387]*z[407] - z[382]*z[412];
    z[425] = z[257]*z[341] + z[356]*z[363] + 0.5*z[381]*z[407] + 0.5*z[384]*z[406] - 0.5*z[382]*z[408] - 0.5*z[383]*z[405];
    z[430] = z[257]*z[356] + z[341]*z[363] + 0.5*z[380]*z[412] + 0.5*z[385]*z[411] + 0.5*z[387]*z[409] + 0.5*z[388]*z[404];
    z[427] = z[385]*z[405] - z[381]*z[409] - 0.25*z[260]*z[341] - 0.25*z[267]*z[356] - 0.25*z[380]*z[408] - 0.25*z[384]*z[404];
    z[426] = z[385]*z[406] + 0.25*z[341]*z[360] + 0.25*z[356]*z[361] - z[382]*z[409] - 0.25*z[380]*z[407] - 0.25*z[383]*z[404];
    z[429] = z[388]*z[405] + 0.25*z[341]*z[362] + 0.25*z[356]*z[359] + 0.25*z[384]*z[411] + 0.25*z[387]*z[408] - z[381]*z[412];
    z[431] = z[297]*z[428] + z[366]*z[410] + z[374]*z[413] + z[376]*z[414] + z[378]*z[415] + z[392]*z[425] + z[393]*z[430] + z[397]*z[427] - z[395]*z[426] - z[400]*z[429];
    z[452] = z[287]*z[386] + z[329]*z[413] + z[341]*z[425] + z[342]*z[428] + z[350]*z[427] + z[356]*z[430] + 4*z[254]*z[365] - z[302]*z[389] - z[309]*z[390] - z[312]*z[391] - z[314]*z[410] - z[336]*z[414] - z[339]*z[415] - z[347]*z[426] - z[354]*z[429];
    z[445] = z[324]*z[428] + z[416]*z[425] + z[417]*z[430] + z[427]*z[441] - z[366]*z[386] - z[374]*z[389] - z[376]*z[390] - z[378]*z[391] - z[426]*z[440] - z[429]*z[442];
    z[491] = z[372]*z[385] + 0.0625*z[260]*z[469] + 0.5*z[276]*z[383] - 0.5*z[384]*z[461] - 0.25*z[257]*z[471] - 0.0625*z[360]*z[468];
    z[496] = z[372]*z[412] + 0.0625*z[267]*z[479] + 0.0625*z[362]*z[480] + 0.0625*z[407]*z[463] - 0.25*z[257]*z[485] - 0.0625*z[408]*z[464];
    z[499] = z[356]*z[372] + 0.125*z[383]*z[479] + 0.125*z[384]*z[480] + 0.125*z[407]*z[468] + 0.125*z[408]*z[469];
    z[502] = 0.125*z[356]*z[464] + 0.125*z[383]*z[485] + 0.125*z[407]*z[474] + 0.25*z[388]*z[480] + 0.25*z[412]*z[469];
    z[501] = z[276]*z[356] - 0.25*z[385]*z[479] - 0.25*z[409]*z[468] - 0.125*z[384]*z[482] - 0.125*z[408]*z[471];
    z[504] = 0.25*z[385]*z[485] + 0.25*z[388]*z[482] + 0.25*z[409]*z[474] + 0.25*z[412]*z[471] + 0.5*z[356]*z[367];
    z[490] = z[363]*z[372] + 0.25*z[267]*z[276] + 0.5*z[257]*z[367] - 0.25*z[362]*z[461] - 0.03125*z[260]*z[464] - 0.03125*z[360]*z[463];
    z[492] = z[372]*z[388] + 0.0625*z[383]*z[463] + 0.25*z[257]*z[474] - 0.0625*z[267]*z[468] - 0.0625*z[362]*z[469] - 0.0625*z[384]*z[464];
    z[493] = z[388]*z[461] + 0.0625*z[267]*z[471] + 0.0625*z[360]*z[474] + 0.25*z[367]*z[383] - 0.25*z[363]*z[469] - 0.125*z[385]*z[464];
    z[494] = z[276]*z[388] + 0.0625*z[362]*z[471] + 0.25*z[363]*z[468] + 0.25*z[367]*z[384] - 0.125*z[385]*z[463] - 0.0625*z[260]*z[474];
    z[495] = z[372]*z[409] + 0.0625*z[360]*z[479] + 0.25*z[257]*z[482] + 0.5*z[276]*z[407] - 0.5*z[408]*z[461] - 0.0625*z[260]*z[480];
    z[497] = z[412]*z[461] + 0.25*z[363]*z[480] + 0.25*z[367]*z[407] - 0.125*z[409]*z[464] - 0.0625*z[267]*z[482] - 0.0625*z[360]*z[485];
    z[498] = z[276]*z[412] + 0.0625*z[260]*z[485] + 0.25*z[367]*z[408] - 0.25*z[363]*z[479] - 0.125*z[409]*z[463] - 0.0625*z[362]*z[482];
    z[500] = z[356]*z[461] + 0.25*z[385]*z[480] + 0.25*z[409]*z[469] - 0.125*z[383]*z[482] - 0.125*z[407]*z[471];
    z[503] = 0.125*z[356]*z[463] + 0.125*z[384]*z[485] + 0.125*z[408]*z[474] - 0.25*z[388]*z[479] - 0.25*z[412]*z[468];
    z[505] = z[287]*z[491] + z[329]*z[496] + z[341]*z[499] + z[342]*z[502] + z[350]*z[501] + z[356]*z[504] + 4*z[254]*z[490] - z[302]*z[492] - z[309]*z[493] - z[312]*z[494] - z[314]*z[495] - z[336]*z[497] - z[339]*z[498] - z[347]*z[500] - z[354]*z[503];
    z[589] = z[284]*z[569] + 0.015625*z[259]*z[290] - 0.25*z[289]*z[557] - 0.125*z[280]*z[566] - 0.0625*z[288]*z[360] - 0.015625*z[282]*z[567];
    z[594] = z[275]*z[577] + 0.015625*z[316]*z[561] + 0.015625*z[317]*z[562] - 0.125*z[278]*z[578] - 0.125*z[284]*z[583] - 0.0625*z[330]*z[360];
    z[597] = z[284]*z[354] + 0.03125*z[290]*z[578] + 0.03125*z[317]*z[567] + 0.25*z[289]*z[577] + 0.25*z[316]*z[566];
    z[600] = z[278]*z[354] + 0.03125*z[317]*z[572] + 0.25*z[303]*z[577] + 0.25*z[330]*z[566] - 0.03125*z[290]*z[583];
    z[599] = -0.25*z[289]*z[580] - 0.25*z[316]*z[569] - 0.125*z[280]*z[354] - 0.03125*z[288]*z[578] - 0.03125*z[315]*z[567];
    z[602] = 0.03125*z[315]*z[572] + 0.25*z[303]*z[580] + 0.25*z[330]*z[569] + 0.5*z[271]*z[354] - 0.03125*z[288]*z[583];
    z[588] = 0.125*z[271]*z[360] + 0.25*z[284]*z[361] - z[275]*z[557] - 0.0625*z[259]*z[278] - 0.0078125*z[280]*z[561] - 0.0078125*z[282]*z[562];
    z[590] = z[275]*z[566] + 0.0625*z[303]*z[360] + 0.125*z[284]*z[572] - 0.125*z[278]*z[567] - 0.015625*z[289]*z[561] - 0.015625*z[290]*z[562];
    z[591] = 0.015625*z[282]*z[572] + 0.015625*z[288]*z[561] + 0.25*z[303]*z[557] + 0.5*z[271]*z[566] - z[278]*z[569] - 0.0625*z[290]*z[361];
    z[592] = 0.015625*z[288]*z[562] + 0.0625*z[271]*z[567] + 0.0625*z[289]*z[361] - z[275]*z[569] - 0.015625*z[259]*z[303] - 0.015625*z[280]*z[572];
    z[593] = z[284]*z[580] + 0.0625*z[315]*z[360] + 0.25*z[316]*z[557] - 0.125*z[280]*z[577] - 0.015625*z[259]*z[317] - 0.015625*z[282]*z[578];
    z[595] = 0.0625*z[317]*z[361] + 0.5*z[271]*z[577] - z[278]*z[580] - 0.25*z[330]*z[557] - 0.015625*z[282]*z[583] - 0.015625*z[315]*z[561];
    z[596] = 0.015625*z[259]*z[330] + 0.015625*z[280]*z[583] + 0.0625*z[271]*z[578] - z[275]*z[580] - 0.0625*z[316]*z[361] - 0.015625*z[315]*z[562];
    z[598] = 0.125*z[282]*z[354] + 0.25*z[290]*z[580] + 0.25*z[317]*z[569] - 0.25*z[288]*z[577] - 0.25*z[315]*z[566];
    z[601] = z[275]*z[354] + 0.03125*z[289]*z[583] + 0.03125*z[303]*z[578] + 0.03125*z[330]*z[567] - 0.03125*z[316]*z[572];
    z[603] = z[287]*z[589] + z[329]*z[594] + z[341]*z[597] + z[342]*z[600] + z[350]*z[599] + z[356]*z[602] + 4*z[254]*z[588] - z[302]*z[590] - z[309]*z[591] - z[312]*z[592] - z[314]*z[593] - z[336]*z[595] - z[339]*z[596] - z[347]*z[598] - z[354]*z[601];
    z[533] = z[284]*z[385] + 0.0625*z[260]*z[290] - 0.25*z[257]*z[288] - 0.0625*z[280]*z[383] - 0.0625*z[282]*z[384] - 0.0625*z[289]*z[360];
    z[538] = z[284]*z[412] + 0.0625*z[267]*z[316] + 0.0625*z[317]*z[362] + 0.5*z[275]*z[407] - 0.5*z[278]*z[408] - 0.25*z[257]*z[330];
    z[541] = z[284]*z[356] + 0.125*z[289]*z[407] + 0.125*z[290]*z[408] + 0.125*z[316]*z[383] + 0.125*z[317]*z[384];
    z[544] = z[278]*z[356] + 0.125*z[303]*z[407] + 0.125*z[330]*z[383] + 0.25*z[290]*z[412] + 0.25*z[317]*z[388];
    z[543] = -0.25*z[289]*z[409] - 0.25*z[316]*z[385] - 0.125*z[280]*z[356] - 0.125*z[288]*z[408] - 0.125*z[315]*z[384];
    z[546] = 0.25*z[288]*z[412] + 0.25*z[303]*z[409] + 0.25*z[315]*z[388] + 0.25*z[330]*z[385] + 0.5*z[271]*z[356];
    z[532] = z[284]*z[363] + 0.5*z[257]*z[271] - 0.25*z[260]*z[278] - 0.25*z[275]*z[360] - 0.03125*z[267]*z[280] - 0.03125*z[282]*z[362];
    z[534] = z[284]*z[388] + 0.25*z[257]*z[303] + 0.5*z[275]*z[383] - 0.5*z[278]*z[384] - 0.0625*z[267]*z[289] - 0.0625*z[290]*z[362];
    z[535] = 0.0625*z[267]*z[288] + 0.0625*z[303]*z[360] + 0.125*z[282]*z[388] + 0.25*z[271]*z[383] - z[278]*z[385] - 0.25*z[290]*z[363];
    z[536] = 0.0625*z[288]*z[362] + 0.25*z[271]*z[384] + 0.25*z[289]*z[363] - z[275]*z[385] - 0.125*z[280]*z[388] - 0.0625*z[260]*z[303];
    z[537] = z[284]*z[409] + 0.0625*z[316]*z[360] + 0.25*z[257]*z[315] - 0.0625*z[260]*z[317] - 0.0625*z[280]*z[407] - 0.0625*z[282]*z[408];
    z[539] = 0.125*z[282]*z[412] + 0.25*z[271]*z[407] + 0.25*z[317]*z[363] - z[278]*z[409] - 0.0625*z[267]*z[315] - 0.0625*z[330]*z[360];
    z[540] = 0.0625*z[260]*z[330] + 0.25*z[271]*z[408] - z[275]*z[409] - 0.25*z[316]*z[363] - 0.125*z[280]*z[412] - 0.0625*z[315]*z[362];
    z[542] = 0.125*z[282]*z[356] + 0.25*z[290]*z[409] + 0.25*z[317]*z[385] - 0.125*z[288]*z[407] - 0.125*z[315]*z[383];
    z[545] = z[275]*z[356] + 0.125*z[303]*z[408] + 0.125*z[330]*z[384] - 0.25*z[289]*z[412] - 0.25*z[316]*z[388];
    z[547] = z[287]*z[533] + z[329]*z[538] + z[341]*z[541] + z[342]*z[544] + z[350]*z[543] + z[356]*z[546] + 4*z[254]*z[532] - z[302]*z[534] - z[309]*z[535] - z[312]*z[536] - z[314]*z[537] - z[336]*z[539] - z[339]*z[540] - z[347]*z[542] - z[354]*z[545];
    z[631] = z[284]*z[294] + 0.015625*z[259]*z[289] + 0.015625*z[280]*z[292] + 0.0625*z[260]*z[288] - 0.25*z[258]*z[290] - 0.125*z[282]*z[291];
    z[636] = 0.0625*z[260]*z[330] + 0.125*z[284]*z[331] - z[278]*z[318] - 0.125*z[275]*z[319] - 0.015625*z[263]*z[317] - 0.015625*z[264]*z[316];
    z[639] = z[284]*z[342] + 0.25*z[290]*z[318] + 0.25*z[291]*z[317] - 0.03125*z[289]*z[319] - 0.03125*z[292]*z[316];
    z[642] = z[278]*z[342] + 0.03125*z[290]*z[331] - 0.03125*z[292]*z[330] - 0.03125*z[303]*z[319] - 0.03125*z[304]*z[317];
    z[641] = -0.25*z[288]*z[318] - 0.25*z[289]*z[321] - 0.25*z[291]*z[315] - 0.25*z[294]*z[316] - 0.125*z[280]*z[342];
    z[644] = 0.03125*z[288]*z[331] + 0.25*z[294]*z[330] + 0.25*z[303]*z[321] + 0.5*z[271]*z[342] - 0.03125*z[304]*z[315];
    z[630] = z[258]*z[278] + 0.0078125*z[263]*z[282] + 0.0078125*z[264]*z[280] + 0.0625*z[259]*z[275] - 0.25*z[267]*z[284] - 0.125*z[260]*z[271];
    z[632] = 0.015625*z[263]*z[290] + 0.015625*z[264]*z[289] - z[278]*z[291] - 0.125*z[275]*z[292] - 0.125*z[284]*z[304] - 0.0625*z[260]*z[303];
    z[633] = 0.0625*z[267]*z[290] - z[278]*z[294] - 0.0625*z[271]*z[292] - 0.015625*z[259]*z[303] - 0.015625*z[264]*z[288] - 0.015625*z[282]*z[304];
    z[634] = 0.015625*z[280]*z[304] + 0.25*z[258]*z[303] + 0.5*z[271]*z[291] - z[275]*z[294] - 0.0625*z[267]*z[289] - 0.015625*z[263]*z[288];
    z[635] = z[284]*z[321] + 0.015625*z[280]*z[319] + 0.25*z[258]*z[317] - 0.125*z[282]*z[318] - 0.0625*z[260]*z[315] - 0.015625*z[259]*z[316];
    z[637] = 0.015625*z[259]*z[330] + 0.015625*z[264]*z[315] + 0.015625*z[282]*z[331] - z[278]*z[321] - 0.0625*z[267]*z[317] - 0.0625*z[271]*z[319];
    z[638] = 0.015625*z[263]*z[315] + 0.0625*z[267]*z[316] + 0.5*z[271]*z[318] - z[275]*z[321] - 0.25*z[258]*z[330] - 0.015625*z[280]*z[331];
    z[640] = 0.03125*z[288]*z[319] + 0.03125*z[292]*z[315] + 0.125*z[282]*z[342] + 0.25*z[290]*z[321] + 0.25*z[294]*z[317];
    z[643] = z[275]*z[342] + 0.03125*z[304]*z[316] + 0.25*z[291]*z[330] + 0.25*z[303]*z[318] - 0.03125*z[289]*z[331];
    z[645] = z[287]*z[631] + z[329]*z[636] + z[341]*z[639] + z[342]*z[642] + z[350]*z[641] + z[356]*z[644] + 4*z[254]*z[630] - z[302]*z[632] - z[309]*z[633] - z[312]*z[634] - z[314]*z[635] - z[336]*z[637] - z[339]*z[638] - z[347]*z[640] - z[354]*z[643];
    z[656] = (z[431]*z[223]+z[452]*z[233]+2*z[445]*z[245]+2*z[505]*z[247]+2*z[603]*z[250]-2*z[547]*z[248]-2*z[645]*z[251])/z[358];
    u3p = 0.5*z[656];
    z[215] = u5*z[135] - u6*z[134];
    z[246] = -T1 - z[215];
    z[252] = z[246]/I1;
    u4p = z[252];
    z[432] = z[356]*z[369] + z[393]*z[406] + 0.5*z[341]*z[370] + 0.5*z[392]*z[407] - z[382]*z[417] - 0.5*z[383]*z[416];
    z[434] = z[341]*z[372] + z[392]*z[409] + 0.5*z[356]*z[367] + 0.5*z[380]*z[417] + 0.5*z[393]*z[404] - z[385]*z[416];
    z[433] = z[356]*z[368] + z[393]*z[405] + 0.5*z[341]*z[371] + 0.5*z[392]*z[408] - z[381]*z[417] - 0.5*z[384]*z[416];
    z[435] = z[297]*z[432] + z[302]*z[425] + z[309]*z[426] + z[312]*z[427] + z[374]*z[418] + z[376]*z[419] + z[378]*z[420] + z[393]*z[434] + 4*z[254]*z[410] - z[400]*z[433];
    z[453] = z[329]*z[418] + z[342]*z[432] + z[356]*z[434] + 4*z[254]*z[373] - z[302]*z[394] - z[309]*z[396] - z[312]*z[398] - z[336]*z[419] - z[339]*z[420] - z[354]*z[433];
    z[446] = z[324]*z[432] + z[336]*z[426] + z[339]*z[427] + z[417]*z[434] - 4*z[254]*z[386] - z[329]*z[425] - z[374]*z[394] - z[376]*z[396] - z[378]*z[398] - z[433]*z[442];
    z[506] = z[356]*z[460] + 0.25*z[393]*z[480] + 0.25*z[417]*z[469] + 0.5*z[302]*z[407] + 0.5*z[329]*z[383];
    z[508] = z[302]*z[409] + z[329]*z[385] + z[356]*z[458] + 0.25*z[393]*z[482] + 0.25*z[417]*z[471];
    z[507] = z[356]*z[459] + 0.5*z[302]*z[408] + 0.5*z[329]*z[384] - 0.25*z[393]*z[479] - 0.25*z[417]*z[468];
    z[509] = z[329]*z[481] + z[342]*z[506] + z[356]*z[508] + 4*z[254]*z[462] - z[302]*z[470] - z[309]*z[472] - z[312]*z[473] - z[336]*z[483] - z[339]*z[484] - z[354]*z[507];
    z[604] = z[269]*z[354] + z[287]*z[577] + z[314]*z[566] + 0.25*z[290]*z[442] + 0.25*z[317]*z[400];
    z[606] = z[287]*z[580] + z[314]*z[569] + 0.25*z[262]*z[354] + 0.25*z[288]*z[442] + 0.25*z[315]*z[400];
    z[605] = z[266]*z[354] + 0.125*z[287]*z[578] + 0.125*z[314]*z[567] - 0.25*z[289]*z[442] - 0.25*z[316]*z[400];
    z[607] = z[329]*z[579] + z[342]*z[604] + z[356]*z[606] + 4*z[254]*z[560] - z[302]*z[568] - z[309]*z[570] - z[312]*z[571] - z[336]*z[581] - z[339]*z[582] - z[354]*z[605];
    z[548] = z[269]*z[356] + 0.25*z[290]*z[417] + 0.25*z[317]*z[393] + 0.5*z[287]*z[407] + 0.5*z[314]*z[383];
    z[550] = z[287]*z[409] + z[314]*z[385] + 0.25*z[262]*z[356] + 0.25*z[288]*z[417] + 0.25*z[315]*z[393];
    z[549] = z[266]*z[356] + 0.5*z[287]*z[408] + 0.5*z[314]*z[384] - 0.25*z[289]*z[417] - 0.25*z[316]*z[393];
    z[551] = z[329]*z[525] + z[342]*z[548] + z[356]*z[550] + 4*z[254]*z[514] - z[302]*z[518] - z[309]*z[519] - z[312]*z[520] - z[336]*z[526] - z[339]*z[527] - z[354]*z[549];
    z[646] = z[269]*z[342] + 0.25*z[290]*z[324] + 0.25*z[297]*z[317] - 0.125*z[287]*z[319] - 0.125*z[292]*z[314];
    z[648] = z[287]*z[321] + z[294]*z[314] + 0.25*z[262]*z[342] + 0.25*z[288]*z[324] + 0.25*z[297]*z[315];
    z[647] = z[266]*z[342] + z[287]*z[318] + z[291]*z[314] - 0.25*z[289]*z[324] - 0.25*z[297]*z[316];
    z[649] = z[329]*z[623] + z[342]*z[646] + z[356]*z[648] + 4*z[254]*z[612] - z[302]*z[616] - z[309]*z[617] - z[312]*z[618] - z[336]*z[624] - z[339]*z[625] - z[354]*z[647];
    z[657] = (z[435]*z[223]+z[453]*z[233]+2*z[446]*z[245]+2*z[509]*z[247]+2*z[607]*z[250]-2*z[551]*z[248]-2*z[649]*z[251])/z[358];
    u5p = -0.5*z[657];
    z[436] = z[284]*z[341] + z[392]*z[412] + 0.5*z[271]*z[356] - z[388]*z[416] - 0.5*z[387]*z[417] - 0.5*z[393]*z[411];
    z[437] = z[287]*z[425] + z[309]*z[428] + z[312]*z[429] + z[376]*z[421] + z[378]*z[422] + z[393]*z[436] + z[395]*z[432] + 4*z[254]*z[413] - z[366]*z[418] - z[397]*z[433];
    z[454] = z[314]*z[418] + z[347]*z[432] + z[356]*z[436] + 4*z[254]*z[375] - z[287]*z[394] - z[309]*z[399] - z[312]*z[401] - z[336]*z[421] - z[339]*z[422] - z[350]*z[433];
    z[447] = z[336]*z[428] + z[339]*z[429] + z[366]*z[394] + z[417]*z[436] + z[432]*z[440] - 4*z[254]*z[389] - z[314]*z[425] - z[376]*z[399] - z[378]*z[401] - z[433]*z[441];
    z[510] = z[302]*z[412] + z[329]*z[388] + 0.25*z[262]*z[356] - 0.25*z[393]*z[485] - 0.25*z[417]*z[474];
    z[511] = z[314]*z[481] + z[347]*z[506] + z[356]*z[510] + 4*z[254]*z[465] - z[287]*z[470] - z[309]*z[475] - z[312]*z[476] - z[336]*z[486] - z[339]*z[487] - z[350]*z[507];
    z[608] = z[256]*z[354] + 0.125*z[314]*z[572] - 0.25*z[303]*z[442] - 0.25*z[330]*z[400] - 0.125*z[287]*z[583];
    z[609] = z[314]*z[579] + z[347]*z[604] + z[356]*z[608] + 4*z[254]*z[563] - z[287]*z[568] - z[309]*z[573] - z[312]*z[574] - z[336]*z[584] - z[339]*z[585] - z[350]*z[605];
    z[552] = z[256]*z[356] + z[287]*z[412] + z[314]*z[388] - 0.25*z[303]*z[417] - 0.25*z[330]*z[393];
    z[553] = z[314]*z[525] + z[347]*z[548] + z[356]*z[552] + 4*z[254]*z[515] - z[287]*z[518] - z[309]*z[521] - z[312]*z[522] - z[336]*z[528] - z[339]*z[529] - z[350]*z[549];
    z[650] = z[256]*z[342] + 0.125*z[287]*z[331] - 0.25*z[297]*z[330] - 0.25*z[303]*z[324] - 0.125*z[304]*z[314];
    z[651] = z[314]*z[623] + z[347]*z[646] + z[356]*z[650] + 4*z[254]*z[613] - z[287]*z[616] - z[309]*z[619] - z[312]*z[620] - z[336]*z[626] - z[339]*z[627] - z[350]*z[647];
    z[658] = (z[437]*z[223]+z[454]*z[233]+2*z[447]*z[245]+2*z[511]*z[247]+2*z[609]*z[250]-2*z[553]*z[248]-2*z[651]*z[251])/z[358];
    u6p = 0.5*z[658];
    z[210] = T1*z[36] + T2*z[42] + T3*z[45];
    z[218] = u8*z[138] - u9*z[137];
    z[249] = z[210] - z[218];
    z[253] = z[249]/I1;
    u7p = z[253];
    z[438] = z[287]*z[426] + z[312]*z[430] + z[378]*z[423] + z[392]*z[432] + z[400]*z[436] + 4*z[254]*z[414] - z[302]*z[428] - z[366]*z[419] - z[374]*z[421] - z[397]*z[434];
    z[455] = z[302]*z[399] + z[314]*z[419] + z[341]*z[432] + z[354]*z[436] + 4*z[254]*z[377] - z[287]*z[396] - z[312]*z[402] - z[329]*z[421] - z[339]*z[423] - z[350]*z[434];
    z[448] = z[329]*z[428] + z[339]*z[430] + z[366]*z[396] + z[374]*z[399] + z[416]*z[432] + z[436]*z[442] - 4*z[254]*z[390] - z[314]*z[426] - z[378]*z[402] - z[434]*z[441];
    z[512] = z[302]*z[475] + z[314]*z[483] + z[341]*z[506] + z[354]*z[510] + 4*z[254]*z[466] - z[287]*z[472] - z[312]*z[477] - z[329]*z[486] - z[339]*z[488] - z[350]*z[508];
    z[610] = z[302]*z[573] + z[314]*z[581] + z[341]*z[604] + z[354]*z[608] + 4*z[254]*z[564] - z[287]*z[570] - z[312]*z[575] - z[329]*z[584] - z[339]*z[586] - z[350]*z[606];
    z[554] = z[302]*z[521] + z[314]*z[526] + z[341]*z[548] + z[354]*z[552] + 4*z[254]*z[516] - z[287]*z[519] - z[312]*z[523] - z[329]*z[528] - z[339]*z[530] - z[350]*z[550];
    z[652] = z[302]*z[619] + z[314]*z[624] + z[341]*z[646] + z[354]*z[650] + 4*z[254]*z[614] - z[287]*z[617] - z[312]*z[621] - z[329]*z[626] - z[339]*z[628] - z[350]*z[648];
    z[659] = (z[438]*z[223]+z[455]*z[233]+2*z[448]*z[245]+2*z[512]*z[247]+2*z[610]*z[250]-2*z[554]*z[248]-2*z[652]*z[251])/z[358];
    u8p = -0.5*z[659];
    z[439] = z[287]*z[427] + z[297]*z[436] + z[392]*z[433] + 4*z[254]*z[415] - z[302]*z[429] - z[309]*z[430] - z[366]*z[420] - z[374]*z[422] - z[376]*z[423] - z[395]*z[434];
    z[456] = z[302]*z[401] + z[309]*z[402] + z[314]*z[420] + z[336]*z[423] + z[341]*z[433] + z[342]*z[436] + 4*z[254]*z[379] - z[287]*z[398] - z[329]*z[422] - z[347]*z[434];
    z[449] = z[324]*z[436] + z[329]*z[429] + z[366]*z[398] + z[374]*z[401] + z[376]*z[402] + z[416]*z[433] - 4*z[254]*z[391] - z[314]*z[427] - z[336]*z[430] - z[434]*z[440];
    z[513] = z[302]*z[476] + z[309]*z[477] + z[314]*z[484] + z[336]*z[488] + z[341]*z[507] + z[342]*z[510] + 4*z[254]*z[467] - z[287]*z[473] - z[329]*z[487] - z[347]*z[508];
    z[611] = z[302]*z[574] + z[309]*z[575] + z[314]*z[582] + z[336]*z[586] + z[341]*z[605] + z[342]*z[608] + 4*z[254]*z[565] - z[287]*z[571] - z[329]*z[585] - z[347]*z[606];
    z[555] = z[302]*z[522] + z[309]*z[523] + z[314]*z[527] + z[336]*z[530] + z[341]*z[549] + z[342]*z[552] + 4*z[254]*z[517] - z[287]*z[520] - z[329]*z[529] - z[347]*z[550];
    z[653] = z[302]*z[620] + z[309]*z[621] + z[314]*z[625] + z[336]*z[628] + z[341]*z[647] + z[342]*z[650] + 4*z[254]*z[615] - z[287]*z[618] - z[329]*z[627] - z[347]*z[648];
    z[660] = (z[439]*z[223]+z[456]*z[233]+2*z[449]*z[245]+2*z[513]*z[247]+2*z[611]*z[250]-2*z[555]*z[248]-2*z[653]*z[251])/z[358];
    u9p = 0.5*z[660];


    # oputput: xdot, ydot, zdot
    return array([q1p, q2p, q3p, q4p, q5p, q6p, q7p, q8p, q9p, u1p, u2p, u3p, u4p, u5p, u6p, u7p, u8p, u9p])