From a1f1f441507a12b6bb4d27ca941c92aa17325209 Mon Sep 17 00:00:00 2001 From: Anne Koelewijn Date: Mon, 27 Jan 2025 15:02:23 +0100 Subject: [PATCH] Adding missing acceleration file and updated plot settings to make them look a bit nicer (#7) --- src/HelperFunctions/plotVarType.m | 2 +- src/model/gait2dc/c_files/acc_al.c | 1364 ++++++++++++++++++++++++++++ 2 files changed, 1365 insertions(+), 1 deletion(-) create mode 100644 src/model/gait2dc/c_files/acc_al.c diff --git a/src/HelperFunctions/plotVarType.m b/src/HelperFunctions/plotVarType.m index 5b5952f..c4f86b8 100644 --- a/src/HelperFunctions/plotVarType.m +++ b/src/HelperFunctions/plotVarType.m @@ -103,7 +103,7 @@ if ~isfield(style.subFigSettings, 'originRight'); style.subFigSettings.originRight = 2; end if ~isfield(style.subFigSettings, 'oneUp'); style.subFigSettings.oneUp = 1.5; end if ~isfield(style.subFigSettings, 'oneRight'); style.subFigSettings.oneRight = 2.5; end - if ~isfield(style.subFigSettings, 'nCol'); style.subFigSettings.nCol = 2; end + if ~isfield(style.subFigSettings, 'nCol'); style.subFigSettings.nCol = 5; end % Get stuff from style struct figureSize = style.figureSize; diff --git a/src/model/gait2dc/c_files/acc_al.c b/src/model/gait2dc/c_files/acc_al.c new file mode 100644 index 0000000..0f2bcb0 --- /dev/null +++ b/src/model/gait2dc/c_files/acc_al.c @@ -0,0 +1,1364 @@ + +#include +#include "gait2dc.h" +void acc_al(param_struct* par, double q[NDOF], double qd[NDOF], double qdd[NDOF], + double acc[42], double dacc_dq[42][NDOF], + double dacc_dqd[42][NDOF], double dacc_dqdd[42][NDOF]) { + + static double z[200]; + const double gravity = 9.81; + double q1 = q[0]; + double q1p = qd[0]; + double q1pp = qdd[0]; + double q2 = q[1]; + double q2p = qd[1]; + double q2pp = qdd[1]; + double q3 = q[2]; + double q3p = qd[2]; + double q3pp = qdd[2]; + double q4 = q[3]; + double q4p = qd[3]; + double q4pp = qdd[3]; + double q5 = q[4]; + double q5p = qd[4]; + double q5pp = qdd[4]; + double q6 = q[5]; + double q6p = qd[5]; + double q6pp = qdd[5]; + double q7 = q[6]; + double q7p = qd[6]; + double q7pp = qdd[6]; + double q8 = q[7]; + double q8p = qd[7]; + double q8pp = qdd[7]; + double q9 = q[8]; + double q9p = qd[8]; + double q9pp = qdd[8]; + z[1] = cos(q3); + z[2] = sin(q3); + z[5] = cos(q4); + z[6] = sin(q4); + z[7] = z[1]*z[5] - z[2]*z[6]; + z[8] = -z[1]*z[6] - z[2]*z[5]; + z[9] = z[1]*z[6] + z[2]*z[5]; + z[11] = q3p + q4p; + z[13] = cos(q5); + z[14] = sin(q5); + z[15] = par->ThighLen*(q3p+q4p); + z[16] = z[11]*z[15]; + z[17] = z[7]*z[13] + z[8]*z[14]; + z[18] = z[8]*z[13] - z[7]*z[14]; + z[19] = z[7]*z[14] + z[9]*z[13]; + z[20] = z[7]*z[13] - z[9]*z[14]; + z[21] = par->ThighLen*z[13]; + z[22] = par->ThighLen*z[14]; + z[24] = z[14]*z[16]; + z[25] = z[13]*z[16]; + z[27] = q3p + q4p + q5p; + z[29] = cos(q6); + z[30] = sin(q6); + z[31] = par->ShankLen + z[21]; + z[32] = par->ShankLen*(q3p+q4p+q5p); + z[33] = z[25] + z[27]*z[32]; + z[34] = z[17]*z[29] + z[18]*z[30]; + z[35] = z[18]*z[29] - z[17]*z[30]; + z[36] = z[19]*z[29] + z[20]*z[30]; + z[37] = z[20]*z[29] - z[19]*z[30]; + z[38] = par->ShankLen*z[29]; + z[39] = z[29]*z[31] - z[22]*z[30]; + z[40] = -z[22]*z[29] - z[30]*z[31]; + z[41] = par->ShankLen*z[30]; + z[46] = z[29]*z[24] + z[30]*z[33]; + z[47] = z[29]*z[33] - z[30]*z[24]; + z[50] = q3p + q4p + q5p + q6p; + z[53] = cos(q7); + z[54] = sin(q7); + z[55] = z[1]*z[53] - z[2]*z[54]; + z[56] = -z[1]*z[54] - z[2]*z[53]; + z[57] = z[1]*z[54] + z[2]*z[53]; + z[59] = q3p + q7p; + z[61] = cos(q8); + z[62] = sin(q8); + z[63] = par->ThighLen*(q3p+q7p); + z[64] = z[59]*z[63]; + z[65] = z[55]*z[61] + z[56]*z[62]; + z[66] = z[56]*z[61] - z[55]*z[62]; + z[67] = z[55]*z[62] + z[57]*z[61]; + z[68] = z[55]*z[61] - z[57]*z[62]; + z[69] = par->ThighLen*z[61]; + z[70] = par->ThighLen*z[62]; + z[72] = z[62]*z[64]; + z[73] = z[61]*z[64]; + z[75] = q3p + q7p + q8p; + z[77] = cos(q9); + z[78] = sin(q9); + z[79] = par->ShankLen + z[69]; + z[80] = par->ShankLen*(q3p+q7p+q8p); + z[81] = z[73] + z[75]*z[80]; + z[82] = z[65]*z[77] + z[66]*z[78]; + z[83] = z[66]*z[77] - z[65]*z[78]; + z[84] = z[67]*z[77] + z[68]*z[78]; + z[85] = z[68]*z[77] - z[67]*z[78]; + z[86] = par->ShankLen*z[77]; + z[87] = z[77]*z[79] - z[70]*z[78]; + z[88] = -z[70]*z[77] - z[78]*z[79]; + z[89] = par->ShankLen*z[78]; + z[94] = z[77]*z[72] + z[78]*z[81]; + z[95] = z[77]*z[81] - z[78]*z[72]; + z[98] = q3p + q7p + q8p + q9p; + z[115] = (q3p+q4p+q5p)*z[27]; + z[134] = (q3p+q7p+q8p)*z[75]; + z[143] = z[2]*z[6] - z[1]*z[5]; + z[144] = z[8]*z[13] + z[14]*z[143]; + z[145] = z[13]*z[143] - z[8]*z[14]; + z[146] = -z[7]*z[14] - z[9]*z[13]; + z[147] = -z[7]*z[13] - z[8]*z[14]; + z[148] = z[29]*z[144] + z[30]*z[145]; + z[149] = z[20]*z[29] + z[30]*z[146]; + z[150] = (z[13]*z[29]-z[14]*z[30])*z[16]; + z[151] = z[18]*z[29] + z[30]*z[147]; + z[152] = par->ThighLen*(z[13]*z[30]+z[14]*z[29]); + z[153] = z[29]*z[145] - z[30]*z[144]; + z[154] = z[29]*z[146] - z[20]*z[30]; + z[155] = (z[13]*z[30]+z[14]*z[29])*z[16]; + z[156] = z[29]*z[147] - z[18]*z[30]; + z[157] = par->ThighLen*(z[13]*z[29]-z[14]*z[30]); + z[158] = -z[19]*z[29] - z[20]*z[30]; + z[159] = -z[29]*z[24] - z[30]*z[33]; + z[160] = -z[17]*z[29] - z[18]*z[30]; + z[161] = z[22]*z[30] - z[29]*z[31]; + z[162] = z[2]*z[54] - z[1]*z[53]; + z[163] = z[56]*z[61] + z[62]*z[162]; + z[164] = z[61]*z[162] - z[56]*z[62]; + z[165] = -z[55]*z[62] - z[57]*z[61]; + z[166] = -z[55]*z[61] - z[56]*z[62]; + z[167] = z[77]*z[163] + z[78]*z[164]; + z[168] = z[68]*z[77] + z[78]*z[165]; + z[169] = (z[61]*z[77]-z[62]*z[78])*z[64]; + z[170] = z[66]*z[77] + z[78]*z[166]; + z[171] = par->ThighLen*(z[61]*z[78]+z[62]*z[77]); + z[172] = z[77]*z[164] - z[78]*z[163]; + z[173] = z[77]*z[165] - z[68]*z[78]; + z[174] = (z[61]*z[78]+z[62]*z[77])*z[64]; + z[175] = z[77]*z[166] - z[66]*z[78]; + z[176] = par->ThighLen*(z[61]*z[77]-z[62]*z[78]); + z[177] = -z[67]*z[77] - z[68]*z[78]; + z[178] = -z[77]*z[72] - z[78]*z[81]; + z[179] = -z[65]*z[77] - z[66]*z[78]; + z[180] = z[70]*z[78] - z[77]*z[79]; + z[181] = z[14]*(z[15]+par->ThighLen*z[11]); + z[182] = q3p + q4p + q5p + z[27]; + z[183] = z[13]*(z[15]+par->ThighLen*z[11]); + z[184] = z[32] + z[183] + par->ShankLen*z[27]; + z[185] = z[29]*z[181] + z[30]*z[184]; + z[186] = z[30]*(z[32]+par->ShankLen*z[27]); + z[187] = z[29]*z[184] - z[30]*z[181]; + z[188] = z[29]*(z[32]+par->ShankLen*z[27]); + z[189] = z[62]*(z[63]+par->ThighLen*z[59]); + z[190] = q3p + q7p + q8p + z[75]; + z[191] = z[61]*(z[63]+par->ThighLen*z[59]); + z[192] = z[80] + z[191] + par->ShankLen*z[75]; + z[193] = z[77]*z[189] + z[78]*z[192]; + z[194] = z[78]*(z[80]+par->ShankLen*z[75]); + z[195] = z[77]*z[192] - z[78]*z[189]; + z[196] = z[77]*(z[80]+par->ShankLen*z[75]); + + + acc[0] = gravity*z[2] + z[1]*q1pp + z[2]*q2pp; + acc[1] = -pow(q3p,2); + acc[2] = -q3pp; + acc[3] = gravity*z[1] + z[1]*q2pp - z[2]*q1pp; + acc[4] = q3pp; + acc[5] = -pow(q3p,2); + acc[6] = gravity*z[9] + z[7]*q1pp + z[9]*q2pp; + acc[7] = -(q3p+q4p)*z[11]; + acc[8] = -q3pp - q4pp; + acc[9] = gravity*z[7] + z[7]*q2pp + z[8]*q1pp; + acc[10] = q3pp + q4pp; + acc[11] = -(q3p+q4p)*z[11]; + acc[12] = gravity*z[19] + z[24] + z[17]*q1pp + z[19]*q2pp + z[21]*q3pp + + z[21]*q4pp; + acc[13] = -z[115]; + acc[14] = -q3pp - q4pp - q5pp; + acc[15] = gravity*z[20] + z[25] + z[18]*q1pp + z[20]*q2pp - z[22]*q3pp - + z[22]*q4pp; + acc[16] = q3pp + q4pp + q5pp; + acc[17] = -z[115]; + acc[18] = gravity*z[36] + z[46] + z[34]*q1pp + z[36]*q2pp + z[38]*q5pp + + z[39]*q3pp + z[39]*q4pp; + acc[19] = -pow(z[50],2); + acc[20] = -q3pp - q4pp - q5pp - q6pp; + acc[21] = gravity*z[37] + z[47] + z[35]*q1pp + z[37]*q2pp + z[40]*q3pp + + z[40]*q4pp - z[41]*q5pp; + acc[22] = q3pp + q4pp + q5pp + q6pp; + acc[23] = -pow(z[50],2); + acc[24] = gravity*z[57] + z[55]*q1pp + z[57]*q2pp; + acc[25] = -(q3p+q7p)*z[59]; + acc[26] = -q3pp - q7pp; + acc[27] = gravity*z[55] + z[55]*q2pp + z[56]*q1pp; + acc[28] = q3pp + q7pp; + acc[29] = -(q3p+q7p)*z[59]; + acc[30] = gravity*z[67] + z[72] + z[65]*q1pp + z[67]*q2pp + z[69]*q3pp + + z[69]*q7pp; + acc[31] = -z[134]; + acc[32] = -q3pp - q7pp - q8pp; + acc[33] = gravity*z[68] + z[73] + z[66]*q1pp + z[68]*q2pp - z[70]*q3pp - + z[70]*q7pp; + acc[34] = q3pp + q7pp + q8pp; + acc[35] = -z[134]; + acc[36] = gravity*z[84] + z[94] + z[82]*q1pp + z[84]*q2pp + z[86]*q8pp + + z[87]*q3pp + z[87]*q7pp; + acc[37] = -pow(z[98],2); + acc[38] = -q3pp - q7pp - q8pp - q9pp; + acc[39] = gravity*z[85] + z[95] + z[83]*q1pp + z[85]*q2pp + z[88]*q3pp + + z[88]*q7pp - z[89]*q8pp; + acc[40] = q3pp + q7pp + q8pp + q9pp; + acc[41] = -pow(z[98],2); + dacc_dq[0][0] = 0; + dacc_dq[0][1] = 0; + dacc_dq[0][2] = gravity*cos(q3) + cos(q3)*q2pp - sin(q3)*q1pp; + dacc_dq[0][3] = 0; + dacc_dq[0][4] = 0; + dacc_dq[0][5] = 0; + dacc_dq[0][6] = 0; + dacc_dq[0][7] = 0; + dacc_dq[0][8] = 0; + dacc_dq[1][0] = 0; + dacc_dq[1][1] = 0; + dacc_dq[1][2] = 0; + dacc_dq[1][3] = 0; + dacc_dq[1][4] = 0; + dacc_dq[1][5] = 0; + dacc_dq[1][6] = 0; + dacc_dq[1][7] = 0; + dacc_dq[1][8] = 0; + dacc_dq[2][0] = 0; + dacc_dq[2][1] = 0; + dacc_dq[2][2] = 0; + dacc_dq[2][3] = 0; + dacc_dq[2][4] = 0; + dacc_dq[2][5] = 0; + dacc_dq[2][6] = 0; + dacc_dq[2][7] = 0; + dacc_dq[2][8] = 0; + dacc_dq[3][0] = 0; + dacc_dq[3][1] = 0; + dacc_dq[3][2] = -gravity*sin(q3) - sin(q3)*q2pp - cos(q3)*q1pp; + dacc_dq[3][3] = 0; + dacc_dq[3][4] = 0; + dacc_dq[3][5] = 0; + dacc_dq[3][6] = 0; + dacc_dq[3][7] = 0; + dacc_dq[3][8] = 0; + dacc_dq[4][0] = 0; + dacc_dq[4][1] = 0; + dacc_dq[4][2] = 0; + dacc_dq[4][3] = 0; + dacc_dq[4][4] = 0; + dacc_dq[4][5] = 0; + dacc_dq[4][6] = 0; + dacc_dq[4][7] = 0; + dacc_dq[4][8] = 0; + dacc_dq[5][0] = 0; + dacc_dq[5][1] = 0; + dacc_dq[5][2] = 0; + dacc_dq[5][3] = 0; + dacc_dq[5][4] = 0; + dacc_dq[5][5] = 0; + dacc_dq[5][6] = 0; + dacc_dq[5][7] = 0; + dacc_dq[5][8] = 0; + dacc_dq[6][0] = 0; + dacc_dq[6][1] = 0; + dacc_dq[6][2] = gravity*z[7] + z[7]*q2pp + z[8]*q1pp; + dacc_dq[6][3] = gravity*z[7] + z[7]*q2pp + z[8]*q1pp; + dacc_dq[6][4] = 0; + dacc_dq[6][5] = 0; + dacc_dq[6][6] = 0; + dacc_dq[6][7] = 0; + dacc_dq[6][8] = 0; + dacc_dq[7][0] = 0; + dacc_dq[7][1] = 0; + dacc_dq[7][2] = 0; + dacc_dq[7][3] = 0; + dacc_dq[7][4] = 0; + dacc_dq[7][5] = 0; + dacc_dq[7][6] = 0; + dacc_dq[7][7] = 0; + dacc_dq[7][8] = 0; + dacc_dq[8][0] = 0; + dacc_dq[8][1] = 0; + dacc_dq[8][2] = 0; + dacc_dq[8][3] = 0; + dacc_dq[8][4] = 0; + dacc_dq[8][5] = 0; + dacc_dq[8][6] = 0; + dacc_dq[8][7] = 0; + dacc_dq[8][8] = 0; + dacc_dq[9][0] = 0; + dacc_dq[9][1] = 0; + dacc_dq[9][2] = gravity*z[8] + z[8]*q2pp + z[143]*q1pp; + dacc_dq[9][3] = gravity*z[8] + z[8]*q2pp + z[143]*q1pp; + dacc_dq[9][4] = 0; + dacc_dq[9][5] = 0; + dacc_dq[9][6] = 0; + dacc_dq[9][7] = 0; + dacc_dq[9][8] = 0; + dacc_dq[10][0] = 0; + dacc_dq[10][1] = 0; + dacc_dq[10][2] = 0; + dacc_dq[10][3] = 0; + dacc_dq[10][4] = 0; + dacc_dq[10][5] = 0; + dacc_dq[10][6] = 0; + dacc_dq[10][7] = 0; + dacc_dq[10][8] = 0; + dacc_dq[11][0] = 0; + dacc_dq[11][1] = 0; + dacc_dq[11][2] = 0; + dacc_dq[11][3] = 0; + dacc_dq[11][4] = 0; + dacc_dq[11][5] = 0; + dacc_dq[11][6] = 0; + dacc_dq[11][7] = 0; + dacc_dq[11][8] = 0; + dacc_dq[12][0] = 0; + dacc_dq[12][1] = 0; + dacc_dq[12][2] = gravity*z[17] + z[17]*q2pp + z[144]*q1pp; + dacc_dq[12][3] = gravity*z[17] + z[17]*q2pp + z[144]*q1pp; + dacc_dq[12][4] = gravity*z[20] + cos(q5)*z[16] + z[18]*q1pp + z[20]*q2pp - + par->ThighLen*sin(q5)*q3pp - par->ThighLen*sin(q5)*q4pp; + dacc_dq[12][5] = 0; + dacc_dq[12][6] = 0; + dacc_dq[12][7] = 0; + dacc_dq[12][8] = 0; + dacc_dq[13][0] = 0; + dacc_dq[13][1] = 0; + dacc_dq[13][2] = 0; + dacc_dq[13][3] = 0; + dacc_dq[13][4] = 0; + dacc_dq[13][5] = 0; + dacc_dq[13][6] = 0; + dacc_dq[13][7] = 0; + dacc_dq[13][8] = 0; + dacc_dq[14][0] = 0; + dacc_dq[14][1] = 0; + dacc_dq[14][2] = 0; + dacc_dq[14][3] = 0; + dacc_dq[14][4] = 0; + dacc_dq[14][5] = 0; + dacc_dq[14][6] = 0; + dacc_dq[14][7] = 0; + dacc_dq[14][8] = 0; + dacc_dq[15][0] = 0; + dacc_dq[15][1] = 0; + dacc_dq[15][2] = gravity*z[18] + z[18]*q2pp + z[145]*q1pp; + dacc_dq[15][3] = gravity*z[18] + z[18]*q2pp + z[145]*q1pp; + dacc_dq[15][4] = gravity*z[146] + z[146]*q2pp + z[147]*q1pp - sin(q5)*z[16] - + par->ThighLen*cos(q5)*q3pp - par->ThighLen*cos(q5)*q4pp; + dacc_dq[15][5] = 0; + dacc_dq[15][6] = 0; + dacc_dq[15][7] = 0; + dacc_dq[15][8] = 0; + dacc_dq[16][0] = 0; + dacc_dq[16][1] = 0; + dacc_dq[16][2] = 0; + dacc_dq[16][3] = 0; + dacc_dq[16][4] = 0; + dacc_dq[16][5] = 0; + dacc_dq[16][6] = 0; + dacc_dq[16][7] = 0; + dacc_dq[16][8] = 0; + dacc_dq[17][0] = 0; + dacc_dq[17][1] = 0; + dacc_dq[17][2] = 0; + dacc_dq[17][3] = 0; + dacc_dq[17][4] = 0; + dacc_dq[17][5] = 0; + dacc_dq[17][6] = 0; + dacc_dq[17][7] = 0; + dacc_dq[17][8] = 0; + dacc_dq[18][0] = 0; + dacc_dq[18][1] = 0; + dacc_dq[18][2] = gravity*z[34] + z[34]*q2pp + z[148]*q1pp; + dacc_dq[18][3] = gravity*z[34] + z[34]*q2pp + z[148]*q1pp; + dacc_dq[18][4] = gravity*z[149] + z[150] + z[149]*q2pp + z[151]*q1pp - + z[152]*q3pp - z[152]*q4pp; + dacc_dq[18][5] = gravity*z[37] + z[47] + z[35]*q1pp + z[37]*q2pp + z[40]* + q3pp + z[40]*q4pp - par->ShankLen*sin(q6)*q5pp; + dacc_dq[18][6] = 0; + dacc_dq[18][7] = 0; + dacc_dq[18][8] = 0; + dacc_dq[19][0] = 0; + dacc_dq[19][1] = 0; + dacc_dq[19][2] = 0; + dacc_dq[19][3] = 0; + dacc_dq[19][4] = 0; + dacc_dq[19][5] = 0; + dacc_dq[19][6] = 0; + dacc_dq[19][7] = 0; + dacc_dq[19][8] = 0; + dacc_dq[20][0] = 0; + dacc_dq[20][1] = 0; + dacc_dq[20][2] = 0; + dacc_dq[20][3] = 0; + dacc_dq[20][4] = 0; + dacc_dq[20][5] = 0; + dacc_dq[20][6] = 0; + dacc_dq[20][7] = 0; + dacc_dq[20][8] = 0; + dacc_dq[21][0] = 0; + dacc_dq[21][1] = 0; + dacc_dq[21][2] = gravity*z[35] + z[35]*q2pp + z[153]*q1pp; + dacc_dq[21][3] = gravity*z[35] + z[35]*q2pp + z[153]*q1pp; + dacc_dq[21][4] = gravity*z[154] + z[154]*q2pp + z[156]*q1pp - z[155] - + z[157]*q3pp - z[157]*q4pp; + dacc_dq[21][5] = gravity*z[158] + z[159] + z[158]*q2pp + z[160]*q1pp + + z[161]*q3pp + z[161]*q4pp - par->ShankLen*cos(q6)*q5pp; + dacc_dq[21][6] = 0; + dacc_dq[21][7] = 0; + dacc_dq[21][8] = 0; + dacc_dq[22][0] = 0; + dacc_dq[22][1] = 0; + dacc_dq[22][2] = 0; + dacc_dq[22][3] = 0; + dacc_dq[22][4] = 0; + dacc_dq[22][5] = 0; + dacc_dq[22][6] = 0; + dacc_dq[22][7] = 0; + dacc_dq[22][8] = 0; + dacc_dq[23][0] = 0; + dacc_dq[23][1] = 0; + dacc_dq[23][2] = 0; + dacc_dq[23][3] = 0; + dacc_dq[23][4] = 0; + dacc_dq[23][5] = 0; + dacc_dq[23][6] = 0; + dacc_dq[23][7] = 0; + dacc_dq[23][8] = 0; + dacc_dq[24][0] = 0; + dacc_dq[24][1] = 0; + dacc_dq[24][2] = gravity*z[55] + z[55]*q2pp + z[56]*q1pp; + dacc_dq[24][3] = 0; + dacc_dq[24][4] = 0; + dacc_dq[24][5] = 0; + dacc_dq[24][6] = gravity*z[55] + z[55]*q2pp + z[56]*q1pp; + dacc_dq[24][7] = 0; + dacc_dq[24][8] = 0; + dacc_dq[25][0] = 0; + dacc_dq[25][1] = 0; + dacc_dq[25][2] = 0; + dacc_dq[25][3] = 0; + dacc_dq[25][4] = 0; + dacc_dq[25][5] = 0; + dacc_dq[25][6] = 0; + dacc_dq[25][7] = 0; + dacc_dq[25][8] = 0; + dacc_dq[26][0] = 0; + dacc_dq[26][1] = 0; + dacc_dq[26][2] = 0; + dacc_dq[26][3] = 0; + dacc_dq[26][4] = 0; + dacc_dq[26][5] = 0; + dacc_dq[26][6] = 0; + dacc_dq[26][7] = 0; + dacc_dq[26][8] = 0; + dacc_dq[27][0] = 0; + dacc_dq[27][1] = 0; + dacc_dq[27][2] = gravity*z[56] + z[56]*q2pp + z[162]*q1pp; + dacc_dq[27][3] = 0; + dacc_dq[27][4] = 0; + dacc_dq[27][5] = 0; + dacc_dq[27][6] = gravity*z[56] + z[56]*q2pp + z[162]*q1pp; + dacc_dq[27][7] = 0; + dacc_dq[27][8] = 0; + dacc_dq[28][0] = 0; + dacc_dq[28][1] = 0; + dacc_dq[28][2] = 0; + dacc_dq[28][3] = 0; + dacc_dq[28][4] = 0; + dacc_dq[28][5] = 0; + dacc_dq[28][6] = 0; + dacc_dq[28][7] = 0; + dacc_dq[28][8] = 0; + dacc_dq[29][0] = 0; + dacc_dq[29][1] = 0; + dacc_dq[29][2] = 0; + dacc_dq[29][3] = 0; + dacc_dq[29][4] = 0; + dacc_dq[29][5] = 0; + dacc_dq[29][6] = 0; + dacc_dq[29][7] = 0; + dacc_dq[29][8] = 0; + dacc_dq[30][0] = 0; + dacc_dq[30][1] = 0; + dacc_dq[30][2] = gravity*z[65] + z[65]*q2pp + z[163]*q1pp; + dacc_dq[30][3] = 0; + dacc_dq[30][4] = 0; + dacc_dq[30][5] = 0; + dacc_dq[30][6] = gravity*z[65] + z[65]*q2pp + z[163]*q1pp; + dacc_dq[30][7] = gravity*z[68] + cos(q8)*z[64] + z[66]*q1pp + z[68]*q2pp - + par->ThighLen*sin(q8)*q3pp - par->ThighLen*sin(q8)*q7pp; + dacc_dq[30][8] = 0; + dacc_dq[31][0] = 0; + dacc_dq[31][1] = 0; + dacc_dq[31][2] = 0; + dacc_dq[31][3] = 0; + dacc_dq[31][4] = 0; + dacc_dq[31][5] = 0; + dacc_dq[31][6] = 0; + dacc_dq[31][7] = 0; + dacc_dq[31][8] = 0; + dacc_dq[32][0] = 0; + dacc_dq[32][1] = 0; + dacc_dq[32][2] = 0; + dacc_dq[32][3] = 0; + dacc_dq[32][4] = 0; + dacc_dq[32][5] = 0; + dacc_dq[32][6] = 0; + dacc_dq[32][7] = 0; + dacc_dq[32][8] = 0; + dacc_dq[33][0] = 0; + dacc_dq[33][1] = 0; + dacc_dq[33][2] = gravity*z[66] + z[66]*q2pp + z[164]*q1pp; + dacc_dq[33][3] = 0; + dacc_dq[33][4] = 0; + dacc_dq[33][5] = 0; + dacc_dq[33][6] = gravity*z[66] + z[66]*q2pp + z[164]*q1pp; + dacc_dq[33][7] = gravity*z[165] + z[165]*q2pp + z[166]*q1pp - sin(q8)*z[64] - + par->ThighLen*cos(q8)*q3pp - par->ThighLen*cos(q8)*q7pp; + dacc_dq[33][8] = 0; + dacc_dq[34][0] = 0; + dacc_dq[34][1] = 0; + dacc_dq[34][2] = 0; + dacc_dq[34][3] = 0; + dacc_dq[34][4] = 0; + dacc_dq[34][5] = 0; + dacc_dq[34][6] = 0; + dacc_dq[34][7] = 0; + dacc_dq[34][8] = 0; + dacc_dq[35][0] = 0; + dacc_dq[35][1] = 0; + dacc_dq[35][2] = 0; + dacc_dq[35][3] = 0; + dacc_dq[35][4] = 0; + dacc_dq[35][5] = 0; + dacc_dq[35][6] = 0; + dacc_dq[35][7] = 0; + dacc_dq[35][8] = 0; + dacc_dq[36][0] = 0; + dacc_dq[36][1] = 0; + dacc_dq[36][2] = gravity*z[82] + z[82]*q2pp + z[167]*q1pp; + dacc_dq[36][3] = 0; + dacc_dq[36][4] = 0; + dacc_dq[36][5] = 0; + dacc_dq[36][6] = gravity*z[82] + z[82]*q2pp + z[167]*q1pp; + dacc_dq[36][7] = gravity*z[168] + z[169] + z[168]*q2pp + z[170]*q1pp - + z[171]*q3pp - z[171]*q7pp; + dacc_dq[36][8] = gravity*z[85] + z[95] + z[83]*q1pp + z[85]*q2pp + z[88]* + q3pp + z[88]*q7pp - par->ShankLen*sin(q9)*q8pp; + dacc_dq[37][0] = 0; + dacc_dq[37][1] = 0; + dacc_dq[37][2] = 0; + dacc_dq[37][3] = 0; + dacc_dq[37][4] = 0; + dacc_dq[37][5] = 0; + dacc_dq[37][6] = 0; + dacc_dq[37][7] = 0; + dacc_dq[37][8] = 0; + dacc_dq[38][0] = 0; + dacc_dq[38][1] = 0; + dacc_dq[38][2] = 0; + dacc_dq[38][3] = 0; + dacc_dq[38][4] = 0; + dacc_dq[38][5] = 0; + dacc_dq[38][6] = 0; + dacc_dq[38][7] = 0; + dacc_dq[38][8] = 0; + dacc_dq[39][0] = 0; + dacc_dq[39][1] = 0; + dacc_dq[39][2] = gravity*z[83] + z[83]*q2pp + z[172]*q1pp; + dacc_dq[39][3] = 0; + dacc_dq[39][4] = 0; + dacc_dq[39][5] = 0; + dacc_dq[39][6] = gravity*z[83] + z[83]*q2pp + z[172]*q1pp; + dacc_dq[39][7] = gravity*z[173] + z[173]*q2pp + z[175]*q1pp - z[174] - + z[176]*q3pp - z[176]*q7pp; + dacc_dq[39][8] = gravity*z[177] + z[178] + z[177]*q2pp + z[179]*q1pp + + z[180]*q3pp + z[180]*q7pp - par->ShankLen*cos(q9)*q8pp; + dacc_dq[40][0] = 0; + dacc_dq[40][1] = 0; + dacc_dq[40][2] = 0; + dacc_dq[40][3] = 0; + dacc_dq[40][4] = 0; + dacc_dq[40][5] = 0; + dacc_dq[40][6] = 0; + dacc_dq[40][7] = 0; + dacc_dq[40][8] = 0; + dacc_dq[41][0] = 0; + dacc_dq[41][1] = 0; + dacc_dq[41][2] = 0; + dacc_dq[41][3] = 0; + dacc_dq[41][4] = 0; + dacc_dq[41][5] = 0; + dacc_dq[41][6] = 0; + dacc_dq[41][7] = 0; + dacc_dq[41][8] = 0; + dacc_dqd[0][0] = 0; + dacc_dqd[0][1] = 0; + dacc_dqd[0][2] = 0; + dacc_dqd[0][3] = 0; + dacc_dqd[0][4] = 0; + dacc_dqd[0][5] = 0; + dacc_dqd[0][6] = 0; + dacc_dqd[0][7] = 0; + dacc_dqd[0][8] = 0; + dacc_dqd[1][0] = 0; + dacc_dqd[1][1] = 0; + dacc_dqd[1][2] = -2*q3p; + dacc_dqd[1][3] = 0; + dacc_dqd[1][4] = 0; + dacc_dqd[1][5] = 0; + dacc_dqd[1][6] = 0; + dacc_dqd[1][7] = 0; + dacc_dqd[1][8] = 0; + dacc_dqd[2][0] = 0; + dacc_dqd[2][1] = 0; + dacc_dqd[2][2] = 0; + dacc_dqd[2][3] = 0; + dacc_dqd[2][4] = 0; + dacc_dqd[2][5] = 0; + dacc_dqd[2][6] = 0; + dacc_dqd[2][7] = 0; + dacc_dqd[2][8] = 0; + dacc_dqd[3][0] = 0; + dacc_dqd[3][1] = 0; + dacc_dqd[3][2] = 0; + dacc_dqd[3][3] = 0; + dacc_dqd[3][4] = 0; + dacc_dqd[3][5] = 0; + dacc_dqd[3][6] = 0; + dacc_dqd[3][7] = 0; + dacc_dqd[3][8] = 0; + dacc_dqd[4][0] = 0; + dacc_dqd[4][1] = 0; + dacc_dqd[4][2] = 0; + dacc_dqd[4][3] = 0; + dacc_dqd[4][4] = 0; + dacc_dqd[4][5] = 0; + dacc_dqd[4][6] = 0; + dacc_dqd[4][7] = 0; + dacc_dqd[4][8] = 0; + dacc_dqd[5][0] = 0; + dacc_dqd[5][1] = 0; + dacc_dqd[5][2] = -2*q3p; + dacc_dqd[5][3] = 0; + dacc_dqd[5][4] = 0; + dacc_dqd[5][5] = 0; + dacc_dqd[5][6] = 0; + dacc_dqd[5][7] = 0; + dacc_dqd[5][8] = 0; + dacc_dqd[6][0] = 0; + dacc_dqd[6][1] = 0; + dacc_dqd[6][2] = 0; + dacc_dqd[6][3] = 0; + dacc_dqd[6][4] = 0; + dacc_dqd[6][5] = 0; + dacc_dqd[6][6] = 0; + dacc_dqd[6][7] = 0; + dacc_dqd[6][8] = 0; + dacc_dqd[7][0] = 0; + dacc_dqd[7][1] = 0; + dacc_dqd[7][2] = -q3p - q4p - z[11]; + dacc_dqd[7][3] = -q3p - q4p - z[11]; + dacc_dqd[7][4] = 0; + dacc_dqd[7][5] = 0; + dacc_dqd[7][6] = 0; + dacc_dqd[7][7] = 0; + dacc_dqd[7][8] = 0; + dacc_dqd[8][0] = 0; + dacc_dqd[8][1] = 0; + dacc_dqd[8][2] = 0; + dacc_dqd[8][3] = 0; + dacc_dqd[8][4] = 0; + dacc_dqd[8][5] = 0; + dacc_dqd[8][6] = 0; + dacc_dqd[8][7] = 0; + dacc_dqd[8][8] = 0; + dacc_dqd[9][0] = 0; + dacc_dqd[9][1] = 0; + dacc_dqd[9][2] = 0; + dacc_dqd[9][3] = 0; + dacc_dqd[9][4] = 0; + dacc_dqd[9][5] = 0; + dacc_dqd[9][6] = 0; + dacc_dqd[9][7] = 0; + dacc_dqd[9][8] = 0; + dacc_dqd[10][0] = 0; + dacc_dqd[10][1] = 0; + dacc_dqd[10][2] = 0; + dacc_dqd[10][3] = 0; + dacc_dqd[10][4] = 0; + dacc_dqd[10][5] = 0; + dacc_dqd[10][6] = 0; + dacc_dqd[10][7] = 0; + dacc_dqd[10][8] = 0; + dacc_dqd[11][0] = 0; + dacc_dqd[11][1] = 0; + dacc_dqd[11][2] = -q3p - q4p - z[11]; + dacc_dqd[11][3] = -q3p - q4p - z[11]; + dacc_dqd[11][4] = 0; + dacc_dqd[11][5] = 0; + dacc_dqd[11][6] = 0; + dacc_dqd[11][7] = 0; + dacc_dqd[11][8] = 0; + dacc_dqd[12][0] = 0; + dacc_dqd[12][1] = 0; + dacc_dqd[12][2] = z[181]; + dacc_dqd[12][3] = z[181]; + dacc_dqd[12][4] = 0; + dacc_dqd[12][5] = 0; + dacc_dqd[12][6] = 0; + dacc_dqd[12][7] = 0; + dacc_dqd[12][8] = 0; + dacc_dqd[13][0] = 0; + dacc_dqd[13][1] = 0; + dacc_dqd[13][2] = -z[182]; + dacc_dqd[13][3] = -z[182]; + dacc_dqd[13][4] = -z[182]; + dacc_dqd[13][5] = 0; + dacc_dqd[13][6] = 0; + dacc_dqd[13][7] = 0; + dacc_dqd[13][8] = 0; + dacc_dqd[14][0] = 0; + dacc_dqd[14][1] = 0; + dacc_dqd[14][2] = 0; + dacc_dqd[14][3] = 0; + dacc_dqd[14][4] = 0; + dacc_dqd[14][5] = 0; + dacc_dqd[14][6] = 0; + dacc_dqd[14][7] = 0; + dacc_dqd[14][8] = 0; + dacc_dqd[15][0] = 0; + dacc_dqd[15][1] = 0; + dacc_dqd[15][2] = z[183]; + dacc_dqd[15][3] = z[183]; + dacc_dqd[15][4] = 0; + dacc_dqd[15][5] = 0; + dacc_dqd[15][6] = 0; + dacc_dqd[15][7] = 0; + dacc_dqd[15][8] = 0; + dacc_dqd[16][0] = 0; + dacc_dqd[16][1] = 0; + dacc_dqd[16][2] = 0; + dacc_dqd[16][3] = 0; + dacc_dqd[16][4] = 0; + dacc_dqd[16][5] = 0; + dacc_dqd[16][6] = 0; + dacc_dqd[16][7] = 0; + dacc_dqd[16][8] = 0; + dacc_dqd[17][0] = 0; + dacc_dqd[17][1] = 0; + dacc_dqd[17][2] = -z[182]; + dacc_dqd[17][3] = -z[182]; + dacc_dqd[17][4] = -z[182]; + dacc_dqd[17][5] = 0; + dacc_dqd[17][6] = 0; + dacc_dqd[17][7] = 0; + dacc_dqd[17][8] = 0; + dacc_dqd[18][0] = 0; + dacc_dqd[18][1] = 0; + dacc_dqd[18][2] = z[185]; + dacc_dqd[18][3] = z[185]; + dacc_dqd[18][4] = z[186]; + dacc_dqd[18][5] = 0; + dacc_dqd[18][6] = 0; + dacc_dqd[18][7] = 0; + dacc_dqd[18][8] = 0; + dacc_dqd[19][0] = 0; + dacc_dqd[19][1] = 0; + dacc_dqd[19][2] = -2*z[50]; + dacc_dqd[19][3] = -2*z[50]; + dacc_dqd[19][4] = -2*z[50]; + dacc_dqd[19][5] = -2*z[50]; + dacc_dqd[19][6] = 0; + dacc_dqd[19][7] = 0; + dacc_dqd[19][8] = 0; + dacc_dqd[20][0] = 0; + dacc_dqd[20][1] = 0; + dacc_dqd[20][2] = 0; + dacc_dqd[20][3] = 0; + dacc_dqd[20][4] = 0; + dacc_dqd[20][5] = 0; + dacc_dqd[20][6] = 0; + dacc_dqd[20][7] = 0; + dacc_dqd[20][8] = 0; + dacc_dqd[21][0] = 0; + dacc_dqd[21][1] = 0; + dacc_dqd[21][2] = z[187]; + dacc_dqd[21][3] = z[187]; + dacc_dqd[21][4] = z[188]; + dacc_dqd[21][5] = 0; + dacc_dqd[21][6] = 0; + dacc_dqd[21][7] = 0; + dacc_dqd[21][8] = 0; + dacc_dqd[22][0] = 0; + dacc_dqd[22][1] = 0; + dacc_dqd[22][2] = 0; + dacc_dqd[22][3] = 0; + dacc_dqd[22][4] = 0; + dacc_dqd[22][5] = 0; + dacc_dqd[22][6] = 0; + dacc_dqd[22][7] = 0; + dacc_dqd[22][8] = 0; + dacc_dqd[23][0] = 0; + dacc_dqd[23][1] = 0; + dacc_dqd[23][2] = -2*z[50]; + dacc_dqd[23][3] = -2*z[50]; + dacc_dqd[23][4] = -2*z[50]; + dacc_dqd[23][5] = -2*z[50]; + dacc_dqd[23][6] = 0; + dacc_dqd[23][7] = 0; + dacc_dqd[23][8] = 0; + dacc_dqd[24][0] = 0; + dacc_dqd[24][1] = 0; + dacc_dqd[24][2] = 0; + dacc_dqd[24][3] = 0; + dacc_dqd[24][4] = 0; + dacc_dqd[24][5] = 0; + dacc_dqd[24][6] = 0; + dacc_dqd[24][7] = 0; + dacc_dqd[24][8] = 0; + dacc_dqd[25][0] = 0; + dacc_dqd[25][1] = 0; + dacc_dqd[25][2] = -q3p - q7p - z[59]; + dacc_dqd[25][3] = 0; + dacc_dqd[25][4] = 0; + dacc_dqd[25][5] = 0; + dacc_dqd[25][6] = -q3p - q7p - z[59]; + dacc_dqd[25][7] = 0; + dacc_dqd[25][8] = 0; + dacc_dqd[26][0] = 0; + dacc_dqd[26][1] = 0; + dacc_dqd[26][2] = 0; + dacc_dqd[26][3] = 0; + dacc_dqd[26][4] = 0; + dacc_dqd[26][5] = 0; + dacc_dqd[26][6] = 0; + dacc_dqd[26][7] = 0; + dacc_dqd[26][8] = 0; + dacc_dqd[27][0] = 0; + dacc_dqd[27][1] = 0; + dacc_dqd[27][2] = 0; + dacc_dqd[27][3] = 0; + dacc_dqd[27][4] = 0; + dacc_dqd[27][5] = 0; + dacc_dqd[27][6] = 0; + dacc_dqd[27][7] = 0; + dacc_dqd[27][8] = 0; + dacc_dqd[28][0] = 0; + dacc_dqd[28][1] = 0; + dacc_dqd[28][2] = 0; + dacc_dqd[28][3] = 0; + dacc_dqd[28][4] = 0; + dacc_dqd[28][5] = 0; + dacc_dqd[28][6] = 0; + dacc_dqd[28][7] = 0; + dacc_dqd[28][8] = 0; + dacc_dqd[29][0] = 0; + dacc_dqd[29][1] = 0; + dacc_dqd[29][2] = -q3p - q7p - z[59]; + dacc_dqd[29][3] = 0; + dacc_dqd[29][4] = 0; + dacc_dqd[29][5] = 0; + dacc_dqd[29][6] = -q3p - q7p - z[59]; + dacc_dqd[29][7] = 0; + dacc_dqd[29][8] = 0; + dacc_dqd[30][0] = 0; + dacc_dqd[30][1] = 0; + dacc_dqd[30][2] = z[189]; + dacc_dqd[30][3] = 0; + dacc_dqd[30][4] = 0; + dacc_dqd[30][5] = 0; + dacc_dqd[30][6] = z[189]; + dacc_dqd[30][7] = 0; + dacc_dqd[30][8] = 0; + dacc_dqd[31][0] = 0; + dacc_dqd[31][1] = 0; + dacc_dqd[31][2] = -z[190]; + dacc_dqd[31][3] = 0; + dacc_dqd[31][4] = 0; + dacc_dqd[31][5] = 0; + dacc_dqd[31][6] = -z[190]; + dacc_dqd[31][7] = -z[190]; + dacc_dqd[31][8] = 0; + dacc_dqd[32][0] = 0; + dacc_dqd[32][1] = 0; + dacc_dqd[32][2] = 0; + dacc_dqd[32][3] = 0; + dacc_dqd[32][4] = 0; + dacc_dqd[32][5] = 0; + dacc_dqd[32][6] = 0; + dacc_dqd[32][7] = 0; + dacc_dqd[32][8] = 0; + dacc_dqd[33][0] = 0; + dacc_dqd[33][1] = 0; + dacc_dqd[33][2] = z[191]; + dacc_dqd[33][3] = 0; + dacc_dqd[33][4] = 0; + dacc_dqd[33][5] = 0; + dacc_dqd[33][6] = z[191]; + dacc_dqd[33][7] = 0; + dacc_dqd[33][8] = 0; + dacc_dqd[34][0] = 0; + dacc_dqd[34][1] = 0; + dacc_dqd[34][2] = 0; + dacc_dqd[34][3] = 0; + dacc_dqd[34][4] = 0; + dacc_dqd[34][5] = 0; + dacc_dqd[34][6] = 0; + dacc_dqd[34][7] = 0; + dacc_dqd[34][8] = 0; + dacc_dqd[35][0] = 0; + dacc_dqd[35][1] = 0; + dacc_dqd[35][2] = -z[190]; + dacc_dqd[35][3] = 0; + dacc_dqd[35][4] = 0; + dacc_dqd[35][5] = 0; + dacc_dqd[35][6] = -z[190]; + dacc_dqd[35][7] = -z[190]; + dacc_dqd[35][8] = 0; + dacc_dqd[36][0] = 0; + dacc_dqd[36][1] = 0; + dacc_dqd[36][2] = z[193]; + dacc_dqd[36][3] = 0; + dacc_dqd[36][4] = 0; + dacc_dqd[36][5] = 0; + dacc_dqd[36][6] = z[193]; + dacc_dqd[36][7] = z[194]; + dacc_dqd[36][8] = 0; + dacc_dqd[37][0] = 0; + dacc_dqd[37][1] = 0; + dacc_dqd[37][2] = -2*z[98]; + dacc_dqd[37][3] = 0; + dacc_dqd[37][4] = 0; + dacc_dqd[37][5] = 0; + dacc_dqd[37][6] = -2*z[98]; + dacc_dqd[37][7] = -2*z[98]; + dacc_dqd[37][8] = -2*z[98]; + dacc_dqd[38][0] = 0; + dacc_dqd[38][1] = 0; + dacc_dqd[38][2] = 0; + dacc_dqd[38][3] = 0; + dacc_dqd[38][4] = 0; + dacc_dqd[38][5] = 0; + dacc_dqd[38][6] = 0; + dacc_dqd[38][7] = 0; + dacc_dqd[38][8] = 0; + dacc_dqd[39][0] = 0; + dacc_dqd[39][1] = 0; + dacc_dqd[39][2] = z[195]; + dacc_dqd[39][3] = 0; + dacc_dqd[39][4] = 0; + dacc_dqd[39][5] = 0; + dacc_dqd[39][6] = z[195]; + dacc_dqd[39][7] = z[196]; + dacc_dqd[39][8] = 0; + dacc_dqd[40][0] = 0; + dacc_dqd[40][1] = 0; + dacc_dqd[40][2] = 0; + dacc_dqd[40][3] = 0; + dacc_dqd[40][4] = 0; + dacc_dqd[40][5] = 0; + dacc_dqd[40][6] = 0; + dacc_dqd[40][7] = 0; + dacc_dqd[40][8] = 0; + dacc_dqd[41][0] = 0; + dacc_dqd[41][1] = 0; + dacc_dqd[41][2] = -2*z[98]; + dacc_dqd[41][3] = 0; + dacc_dqd[41][4] = 0; + dacc_dqd[41][5] = 0; + dacc_dqd[41][6] = -2*z[98]; + dacc_dqd[41][7] = -2*z[98]; + dacc_dqd[41][8] = -2*z[98]; + dacc_dqdd[0][0] = z[1]; + dacc_dqdd[0][1] = z[2]; + dacc_dqdd[0][2] = 0; + dacc_dqdd[0][3] = 0; + dacc_dqdd[0][4] = 0; + dacc_dqdd[0][5] = 0; + dacc_dqdd[0][6] = 0; + dacc_dqdd[0][7] = 0; + dacc_dqdd[0][8] = 0; + dacc_dqdd[1][0] = 0; + dacc_dqdd[1][1] = 0; + dacc_dqdd[1][2] = 0; + dacc_dqdd[1][3] = 0; + dacc_dqdd[1][4] = 0; + dacc_dqdd[1][5] = 0; + dacc_dqdd[1][6] = 0; + dacc_dqdd[1][7] = 0; + dacc_dqdd[1][8] = 0; + dacc_dqdd[2][0] = 0; + dacc_dqdd[2][1] = 0; + dacc_dqdd[2][2] = -1; + dacc_dqdd[2][3] = 0; + dacc_dqdd[2][4] = 0; + dacc_dqdd[2][5] = 0; + dacc_dqdd[2][6] = 0; + dacc_dqdd[2][7] = 0; + dacc_dqdd[2][8] = 0; + dacc_dqdd[3][0] = -z[2]; + dacc_dqdd[3][1] = z[1]; + dacc_dqdd[3][2] = 0; + dacc_dqdd[3][3] = 0; + dacc_dqdd[3][4] = 0; + dacc_dqdd[3][5] = 0; + dacc_dqdd[3][6] = 0; + dacc_dqdd[3][7] = 0; + dacc_dqdd[3][8] = 0; + dacc_dqdd[4][0] = 0; + dacc_dqdd[4][1] = 0; + dacc_dqdd[4][2] = 1; + dacc_dqdd[4][3] = 0; + dacc_dqdd[4][4] = 0; + dacc_dqdd[4][5] = 0; + dacc_dqdd[4][6] = 0; + dacc_dqdd[4][7] = 0; + dacc_dqdd[4][8] = 0; + dacc_dqdd[5][0] = 0; + dacc_dqdd[5][1] = 0; + dacc_dqdd[5][2] = 0; + dacc_dqdd[5][3] = 0; + dacc_dqdd[5][4] = 0; + dacc_dqdd[5][5] = 0; + dacc_dqdd[5][6] = 0; + dacc_dqdd[5][7] = 0; + dacc_dqdd[5][8] = 0; + dacc_dqdd[6][0] = z[7]; + dacc_dqdd[6][1] = z[9]; + dacc_dqdd[6][2] = 0; + dacc_dqdd[6][3] = 0; + dacc_dqdd[6][4] = 0; + dacc_dqdd[6][5] = 0; + dacc_dqdd[6][6] = 0; + dacc_dqdd[6][7] = 0; + dacc_dqdd[6][8] = 0; + dacc_dqdd[7][0] = 0; + dacc_dqdd[7][1] = 0; + dacc_dqdd[7][2] = 0; + dacc_dqdd[7][3] = 0; + dacc_dqdd[7][4] = 0; + dacc_dqdd[7][5] = 0; + dacc_dqdd[7][6] = 0; + dacc_dqdd[7][7] = 0; + dacc_dqdd[7][8] = 0; + dacc_dqdd[8][0] = 0; + dacc_dqdd[8][1] = 0; + dacc_dqdd[8][2] = -1; + dacc_dqdd[8][3] = -1; + dacc_dqdd[8][4] = 0; + dacc_dqdd[8][5] = 0; + dacc_dqdd[8][6] = 0; + dacc_dqdd[8][7] = 0; + dacc_dqdd[8][8] = 0; + dacc_dqdd[9][0] = z[8]; + dacc_dqdd[9][1] = z[7]; + dacc_dqdd[9][2] = 0; + dacc_dqdd[9][3] = 0; + dacc_dqdd[9][4] = 0; + dacc_dqdd[9][5] = 0; + dacc_dqdd[9][6] = 0; + dacc_dqdd[9][7] = 0; + dacc_dqdd[9][8] = 0; + dacc_dqdd[10][0] = 0; + dacc_dqdd[10][1] = 0; + dacc_dqdd[10][2] = 1; + dacc_dqdd[10][3] = 1; + dacc_dqdd[10][4] = 0; + dacc_dqdd[10][5] = 0; + dacc_dqdd[10][6] = 0; + dacc_dqdd[10][7] = 0; + dacc_dqdd[10][8] = 0; + dacc_dqdd[11][0] = 0; + dacc_dqdd[11][1] = 0; + dacc_dqdd[11][2] = 0; + dacc_dqdd[11][3] = 0; + dacc_dqdd[11][4] = 0; + dacc_dqdd[11][5] = 0; + dacc_dqdd[11][6] = 0; + dacc_dqdd[11][7] = 0; + dacc_dqdd[11][8] = 0; + dacc_dqdd[12][0] = z[17]; + dacc_dqdd[12][1] = z[19]; + dacc_dqdd[12][2] = z[21]; + dacc_dqdd[12][3] = z[21]; + dacc_dqdd[12][4] = 0; + dacc_dqdd[12][5] = 0; + dacc_dqdd[12][6] = 0; + dacc_dqdd[12][7] = 0; + dacc_dqdd[12][8] = 0; + dacc_dqdd[13][0] = 0; + dacc_dqdd[13][1] = 0; + dacc_dqdd[13][2] = 0; + dacc_dqdd[13][3] = 0; + dacc_dqdd[13][4] = 0; + dacc_dqdd[13][5] = 0; + dacc_dqdd[13][6] = 0; + dacc_dqdd[13][7] = 0; + dacc_dqdd[13][8] = 0; + dacc_dqdd[14][0] = 0; + dacc_dqdd[14][1] = 0; + dacc_dqdd[14][2] = -1; + dacc_dqdd[14][3] = -1; + dacc_dqdd[14][4] = -1; + dacc_dqdd[14][5] = 0; + dacc_dqdd[14][6] = 0; + dacc_dqdd[14][7] = 0; + dacc_dqdd[14][8] = 0; + dacc_dqdd[15][0] = z[18]; + dacc_dqdd[15][1] = z[20]; + dacc_dqdd[15][2] = -z[22]; + dacc_dqdd[15][3] = -z[22]; + dacc_dqdd[15][4] = 0; + dacc_dqdd[15][5] = 0; + dacc_dqdd[15][6] = 0; + dacc_dqdd[15][7] = 0; + dacc_dqdd[15][8] = 0; + dacc_dqdd[16][0] = 0; + dacc_dqdd[16][1] = 0; + dacc_dqdd[16][2] = 1; + dacc_dqdd[16][3] = 1; + dacc_dqdd[16][4] = 1; + dacc_dqdd[16][5] = 0; + dacc_dqdd[16][6] = 0; + dacc_dqdd[16][7] = 0; + dacc_dqdd[16][8] = 0; + dacc_dqdd[17][0] = 0; + dacc_dqdd[17][1] = 0; + dacc_dqdd[17][2] = 0; + dacc_dqdd[17][3] = 0; + dacc_dqdd[17][4] = 0; + dacc_dqdd[17][5] = 0; + dacc_dqdd[17][6] = 0; + dacc_dqdd[17][7] = 0; + dacc_dqdd[17][8] = 0; + dacc_dqdd[18][0] = z[34]; + dacc_dqdd[18][1] = z[36]; + dacc_dqdd[18][2] = z[39]; + dacc_dqdd[18][3] = z[39]; + dacc_dqdd[18][4] = z[38]; + dacc_dqdd[18][5] = 0; + dacc_dqdd[18][6] = 0; + dacc_dqdd[18][7] = 0; + dacc_dqdd[18][8] = 0; + dacc_dqdd[19][0] = 0; + dacc_dqdd[19][1] = 0; + dacc_dqdd[19][2] = 0; + dacc_dqdd[19][3] = 0; + dacc_dqdd[19][4] = 0; + dacc_dqdd[19][5] = 0; + dacc_dqdd[19][6] = 0; + dacc_dqdd[19][7] = 0; + dacc_dqdd[19][8] = 0; + dacc_dqdd[20][0] = 0; + dacc_dqdd[20][1] = 0; + dacc_dqdd[20][2] = -1; + dacc_dqdd[20][3] = -1; + dacc_dqdd[20][4] = -1; + dacc_dqdd[20][5] = -1; + dacc_dqdd[20][6] = 0; + dacc_dqdd[20][7] = 0; + dacc_dqdd[20][8] = 0; + dacc_dqdd[21][0] = z[35]; + dacc_dqdd[21][1] = z[37]; + dacc_dqdd[21][2] = z[40]; + dacc_dqdd[21][3] = z[40]; + dacc_dqdd[21][4] = -z[41]; + dacc_dqdd[21][5] = 0; + dacc_dqdd[21][6] = 0; + dacc_dqdd[21][7] = 0; + dacc_dqdd[21][8] = 0; + dacc_dqdd[22][0] = 0; + dacc_dqdd[22][1] = 0; + dacc_dqdd[22][2] = 1; + dacc_dqdd[22][3] = 1; + dacc_dqdd[22][4] = 1; + dacc_dqdd[22][5] = 1; + dacc_dqdd[22][6] = 0; + dacc_dqdd[22][7] = 0; + dacc_dqdd[22][8] = 0; + dacc_dqdd[23][0] = 0; + dacc_dqdd[23][1] = 0; + dacc_dqdd[23][2] = 0; + dacc_dqdd[23][3] = 0; + dacc_dqdd[23][4] = 0; + dacc_dqdd[23][5] = 0; + dacc_dqdd[23][6] = 0; + dacc_dqdd[23][7] = 0; + dacc_dqdd[23][8] = 0; + dacc_dqdd[24][0] = z[55]; + dacc_dqdd[24][1] = z[57]; + dacc_dqdd[24][2] = 0; + dacc_dqdd[24][3] = 0; + dacc_dqdd[24][4] = 0; + dacc_dqdd[24][5] = 0; + dacc_dqdd[24][6] = 0; + dacc_dqdd[24][7] = 0; + dacc_dqdd[24][8] = 0; + dacc_dqdd[25][0] = 0; + dacc_dqdd[25][1] = 0; + dacc_dqdd[25][2] = 0; + dacc_dqdd[25][3] = 0; + dacc_dqdd[25][4] = 0; + dacc_dqdd[25][5] = 0; + dacc_dqdd[25][6] = 0; + dacc_dqdd[25][7] = 0; + dacc_dqdd[25][8] = 0; + dacc_dqdd[26][0] = 0; + dacc_dqdd[26][1] = 0; + dacc_dqdd[26][2] = -1; + dacc_dqdd[26][3] = 0; + dacc_dqdd[26][4] = 0; + dacc_dqdd[26][5] = 0; + dacc_dqdd[26][6] = -1; + dacc_dqdd[26][7] = 0; + dacc_dqdd[26][8] = 0; + dacc_dqdd[27][0] = z[56]; + dacc_dqdd[27][1] = z[55]; + dacc_dqdd[27][2] = 0; + dacc_dqdd[27][3] = 0; + dacc_dqdd[27][4] = 0; + dacc_dqdd[27][5] = 0; + dacc_dqdd[27][6] = 0; + dacc_dqdd[27][7] = 0; + dacc_dqdd[27][8] = 0; + dacc_dqdd[28][0] = 0; + dacc_dqdd[28][1] = 0; + dacc_dqdd[28][2] = 1; + dacc_dqdd[28][3] = 0; + dacc_dqdd[28][4] = 0; + dacc_dqdd[28][5] = 0; + dacc_dqdd[28][6] = 1; + dacc_dqdd[28][7] = 0; + dacc_dqdd[28][8] = 0; + dacc_dqdd[29][0] = 0; + dacc_dqdd[29][1] = 0; + dacc_dqdd[29][2] = 0; + dacc_dqdd[29][3] = 0; + dacc_dqdd[29][4] = 0; + dacc_dqdd[29][5] = 0; + dacc_dqdd[29][6] = 0; + dacc_dqdd[29][7] = 0; + dacc_dqdd[29][8] = 0; + dacc_dqdd[30][0] = z[65]; + dacc_dqdd[30][1] = z[67]; + dacc_dqdd[30][2] = z[69]; + dacc_dqdd[30][3] = 0; + dacc_dqdd[30][4] = 0; + dacc_dqdd[30][5] = 0; + dacc_dqdd[30][6] = z[69]; + dacc_dqdd[30][7] = 0; + dacc_dqdd[30][8] = 0; + dacc_dqdd[31][0] = 0; + dacc_dqdd[31][1] = 0; + dacc_dqdd[31][2] = 0; + dacc_dqdd[31][3] = 0; + dacc_dqdd[31][4] = 0; + dacc_dqdd[31][5] = 0; + dacc_dqdd[31][6] = 0; + dacc_dqdd[31][7] = 0; + dacc_dqdd[31][8] = 0; + dacc_dqdd[32][0] = 0; + dacc_dqdd[32][1] = 0; + dacc_dqdd[32][2] = -1; + dacc_dqdd[32][3] = 0; + dacc_dqdd[32][4] = 0; + dacc_dqdd[32][5] = 0; + dacc_dqdd[32][6] = -1; + dacc_dqdd[32][7] = -1; + dacc_dqdd[32][8] = 0; + dacc_dqdd[33][0] = z[66]; + dacc_dqdd[33][1] = z[68]; + dacc_dqdd[33][2] = -z[70]; + dacc_dqdd[33][3] = 0; + dacc_dqdd[33][4] = 0; + dacc_dqdd[33][5] = 0; + dacc_dqdd[33][6] = -z[70]; + dacc_dqdd[33][7] = 0; + dacc_dqdd[33][8] = 0; + dacc_dqdd[34][0] = 0; + dacc_dqdd[34][1] = 0; + dacc_dqdd[34][2] = 1; + dacc_dqdd[34][3] = 0; + dacc_dqdd[34][4] = 0; + dacc_dqdd[34][5] = 0; + dacc_dqdd[34][6] = 1; + dacc_dqdd[34][7] = 1; + dacc_dqdd[34][8] = 0; + dacc_dqdd[35][0] = 0; + dacc_dqdd[35][1] = 0; + dacc_dqdd[35][2] = 0; + dacc_dqdd[35][3] = 0; + dacc_dqdd[35][4] = 0; + dacc_dqdd[35][5] = 0; + dacc_dqdd[35][6] = 0; + dacc_dqdd[35][7] = 0; + dacc_dqdd[35][8] = 0; + dacc_dqdd[36][0] = z[82]; + dacc_dqdd[36][1] = z[84]; + dacc_dqdd[36][2] = z[87]; + dacc_dqdd[36][3] = 0; + dacc_dqdd[36][4] = 0; + dacc_dqdd[36][5] = 0; + dacc_dqdd[36][6] = z[87]; + dacc_dqdd[36][7] = z[86]; + dacc_dqdd[36][8] = 0; + dacc_dqdd[37][0] = 0; + dacc_dqdd[37][1] = 0; + dacc_dqdd[37][2] = 0; + dacc_dqdd[37][3] = 0; + dacc_dqdd[37][4] = 0; + dacc_dqdd[37][5] = 0; + dacc_dqdd[37][6] = 0; + dacc_dqdd[37][7] = 0; + dacc_dqdd[37][8] = 0; + dacc_dqdd[38][0] = 0; + dacc_dqdd[38][1] = 0; + dacc_dqdd[38][2] = -1; + dacc_dqdd[38][3] = 0; + dacc_dqdd[38][4] = 0; + dacc_dqdd[38][5] = 0; + dacc_dqdd[38][6] = -1; + dacc_dqdd[38][7] = -1; + dacc_dqdd[38][8] = -1; + dacc_dqdd[39][0] = z[83]; + dacc_dqdd[39][1] = z[85]; + dacc_dqdd[39][2] = z[88]; + dacc_dqdd[39][3] = 0; + dacc_dqdd[39][4] = 0; + dacc_dqdd[39][5] = 0; + dacc_dqdd[39][6] = z[88]; + dacc_dqdd[39][7] = -z[89]; + dacc_dqdd[39][8] = 0; + dacc_dqdd[40][0] = 0; + dacc_dqdd[40][1] = 0; + dacc_dqdd[40][2] = 1; + dacc_dqdd[40][3] = 0; + dacc_dqdd[40][4] = 0; + dacc_dqdd[40][5] = 0; + dacc_dqdd[40][6] = 1; + dacc_dqdd[40][7] = 1; + dacc_dqdd[40][8] = 1; + dacc_dqdd[41][0] = 0; + dacc_dqdd[41][1] = 0; + dacc_dqdd[41][2] = 0; + dacc_dqdd[41][3] = 0; + dacc_dqdd[41][4] = 0; + dacc_dqdd[41][5] = 0; + dacc_dqdd[41][6] = 0; + dacc_dqdd[41][7] = 0; + dacc_dqdd[41][8] = 0; + +}