From 812435fc8395a9ca9b2c1f5d3b3fd43dabd2ffd5 Mon Sep 17 00:00:00 2001 From: Ton van den Bogert Date: Tue, 28 Jan 2025 18:14:46 -0500 Subject: [PATCH] Removed finite difference code from sumuAccGyro.m It now uses the symbolic derivatives generated by Autolev. Result was not affected, optimization ran a little faster. acc_al.c was removed because it is replaced by gait2dc_acc_al.c --- src/model/gait2dc/@Gait2dc/simuAccGyro.m | 58 +- src/model/gait2dc/c_files/acc_al.c | 1364 ---------------------- 2 files changed, 13 insertions(+), 1409 deletions(-) delete mode 100644 src/model/gait2dc/c_files/acc_al.c diff --git a/src/model/gait2dc/@Gait2dc/simuAccGyro.m b/src/model/gait2dc/@Gait2dc/simuAccGyro.m index 8560c6f..911442c 100644 --- a/src/model/gait2dc/@Gait2dc/simuAccGyro.m +++ b/src/model/gait2dc/@Gait2dc/simuAccGyro.m @@ -55,7 +55,7 @@ error('The following segments are not defined in the model: %s', sprintf('%s ', nonExistingSegments{:})); end -% Get indices of acc and gyro in variables tabale +% Get indices of acc and gyro in variables table if nargin < 7 idxAcc = find(ismember(variables.type, 'acc'))'; end @@ -66,7 +66,6 @@ nGyro = numel(idxGyro); %number of gyroscope variables nVars = length(idxSegment); % much faster then size(variables, 1) or height(variables) - % Initialize the outputs s = zeros(nVars, 1); if (nargout > 1) @@ -76,53 +75,22 @@ end %% Accelerometer -% determine further values for Accelerometer tracking +% If we have accelerometers, determine the 42 accelerometer model coefficients +% (7 segments x 6 model terms), and their Jacobians +% An accelerometer placed at local coordinates (xpos,ypos) on a body segment +% produces these signals (in local reference frame): +% Sx = a1 + a2*xpos + a3*ypos +% Sy = a4 + a5*xpos + a6*ypos +% xpos,ypos are measured relative to the proximal joint. if nAcc > 0 - hh = 1e-7; - % determine the 42 accelerometer model coefficients (7 segments x 6 model terms), and their Jacobians - if (0) - if nargout > 1 - [a, da_dq, da_dqd, da_dqdd] = gait2dc('Accelerometer',q,qd,qdd); - else - a = gait2dc('Accelerometer',q,qd,qdd); - end + if nargout > 1 + [a, da_dq, da_dqd, da_dqdd] = gait2dc('Accelerometer',q,qd,qdd); else a = gait2dc('Accelerometer',q,qd,qdd); - - if nargout > 1 - % The three Jacobians of a are calculated here by finite differences, because we have a crash if we - % get them from gait2dc.mexw32. This needs to be looked - % at! --> No crash at the moment - da_dq = zeros(42,obj.nDofs); - da_dqd = zeros(42,obj.nDofs); - da_dqdd = zeros(42,obj.nDofs); - - for k = 1:obj.nDofs - tt = q(k); - q(k) = q(k) + hh; - da_dq(:,k) = (gait2dc('Accelerometer',q,qd,qdd) - a) / hh; - q(k) = tt; - - tt = qd(k); - qd(k) = qd(k) + hh; - da_dqd(:,k) = (gait2dc('Accelerometer',q,qd,qdd) - a) / hh; - qd(k) = tt; - - tt = qdd(k); - qdd(k) = qdd(k) + hh; - da_dqdd(:,k) = (gait2dc('Accelerometer',q,qd,qdd) - a) / hh; - qdd(k) = tt; - end - da_dq = sparse(da_dq); - da_dqd = sparse(da_dqd); - da_dqdd = sparse(da_dqdd); - end - end - + end end - -% simulate accelerometer signal +% simulate accelerometer signals if ~isempty(idxAcc) for iVar = idxAcc @@ -137,7 +105,7 @@ % accelerometer signal s value = a(ia); s(iVar) = c*value; - % Jacobians ds/dq, ds/dqd and ds/dh + % Jacobians ds/dq, ds/dqd and ds/dqdd if nargout > 1 ds_dq(iVar, :) = c*da_dq(ia,:); % because ds_dq = c*da_dq(ia,:); ds_dqd(iVar, :) = c*da_dqd(ia,:); % because ds_dqd = c*da_dqd(ia,:); diff --git a/src/model/gait2dc/c_files/acc_al.c b/src/model/gait2dc/c_files/acc_al.c deleted file mode 100644 index 0f2bcb0..0000000 --- a/src/model/gait2dc/c_files/acc_al.c +++ /dev/null @@ -1,1364 +0,0 @@ - -#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; - -}