diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..4e2c163 --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +model/rowerdynamics.m +*.asv +model/tmp.m +model/rowerdynamics.m +model/rowerdynamics.m \ No newline at end of file diff --git a/Setup.txt b/Setup.txt new file mode 100644 index 0000000..16fb2fe --- /dev/null +++ b/Setup.txt @@ -0,0 +1,9 @@ +In order to perform the optimization code the "main.m" should be ran + +-"problem.nconpath" represent the number of path constraints (currently = 1). +-"optimize.m" is coded in a way that it can be increased to 2 also +-To start it is better to set problem.Wtrack =1 while problem.Weffort =0. + The reason is we are sure that the constaraints are satisfied in tracking + problem. In addition, when using only tracking, the optimization should + be solved within few iterations + -The rest can be the way they are. diff --git a/data/Simresult.mat b/data/Simresult.mat index 4ff0a93..4b77108 100644 Binary files a/data/Simresult.mat and b/data/Simresult.mat differ diff --git a/data/Simresult2.mat b/data/Simresult2.mat new file mode 100644 index 0000000..7f22d03 Binary files /dev/null and b/data/Simresult2.mat differ diff --git a/main.m b/main.m deleted file mode 100644 index bedd033..0000000 --- a/main.m +++ /dev/null @@ -1,29 +0,0 @@ -function main - % performs all the optimizations and produces the plots and tables - close all - clc - clear - global result problem - -% Add that folder plus all subfolders to the path. -% mkdir('optimizer') -% addpath(genpath('optimizer')) -% addpath(genpath('model')) -% addpath(genpath('tools')) -% addpath(genpath('data')) - - - problem.initialguess = 'sim'; - problem.N = 70; % number of collocation points -% problem.initialguess = 'midpoint'; -% problem.N = 4; % number of collocation points -% model.task.Lmin = 0.2077; % min and max of cable lenght from the data -% model.Lmax.Lmin = 1.1288; - problem.discretization = 'BE'; % Backward Euler(BE) or Midpoint Euler(ME) discretization -% problem.cabletask = 1; - problem.tracking = 3; % if 1 tracking the angles only else track all states (but flywheel velocity) - problem.Wtrack = 0; - problem.Weffort = 1; - result = optimize(problem); - save 'rowingtest' result -end diff --git a/main/Cons.mat b/main/Cons.mat new file mode 100644 index 0000000..01620ed Binary files /dev/null and b/main/Cons.mat differ diff --git a/main/main.m b/main/main.m new file mode 100644 index 0000000..3248a03 --- /dev/null +++ b/main/main.m @@ -0,0 +1,47 @@ +function main + % performs all the optimizations and produces the plots and tables + close all + clc + clear global + global result problem + + % Add that folder plus all subfolders to the path. + % mkdir('optimizer') + addpath(genpath('optimizer')) + addpath(genpath('model')) + addpath(genpath('tools')) + addpath(genpath('data')) + + problem.initialguess = 'sim';%pu th efilename here + problem.cgain =1; % scaling factor for path constraint + problem.dyngain = 1; % scaling factor for non path constraint + problem.np =1; % on/off switch for constraints + problem.N = 70; % number of collocation points +% problem.Reqpower = 0.0976; % we can define the woring power here + %( we can either use power or path constraint-so if using problem.Reqpower make sure that problem.nconpath = 0) + if problem.np == 1 + problem.task.Lmin = 0.2077; % min and max of cable lenght from the data + problem.task.Lmax = 1.1288; + % task constraints---> when increased even by 1 either for cable for wrist position, the IPOPT cannot solve the + % problem anymore + problem.cablecnst = 2; % number of constraints for task when doing predictive simulation (cable length) Lmin & Lmax + problem.nconpath = 1; % Zwrist constraint (depends on the number of nodes can be varies between 0 and 2 for now) + % when increasing the number of path constraint to 2 IPOPT connot + % solve it anymore! + else + problem.ntask = 0; + end + + + problem.component = 'spring'; % choose if we want to use damper or spring + problem.discretization = 'BE'; % Backward Euler(BE) or Midpoint Euler(ME) discretization + problem.tracking = 3; % if 1 tracking the angles only else track all states (but flywheel velocity) + % objective function gains (Wtrack=1 for the tracking and Weffort=1 for predictive simulation) + problem.Wtrack =0; + problem.Weffort =1; + % tracking problem:when problem.Wtrack =1 and problem.Weffort =0 works perfectly; + % peredictive simulation problem: when problem.Wtrack =0 and problem.Weffort =1 works perfectly; + % for now we only use predictive simulation + result = optimize(problem); + save 'rowingtest' result +end diff --git a/main/rowingtest.mat b/main/rowingtest.mat new file mode 100644 index 0000000..538120c Binary files /dev/null and b/main/rowingtest.mat differ diff --git a/model/.gitignore b/model/.gitignore new file mode 100644 index 0000000..d6ce631 --- /dev/null +++ b/model/.gitignore @@ -0,0 +1,2 @@ +/rowerdynamics.m +/tmp.avi diff --git a/model/animate.m b/model/animate.m index ac8350e..d617977 100644 --- a/model/animate.m +++ b/model/animate.m @@ -19,7 +19,7 @@ function animate(q, par) for i = 1:nframes % use the rowerdynamics function to get the stick figure % coordinates at frame i - [~,~,~,~,~,s] = rowerdynamics(q(i,:)',qd,qdd,xfw,par); + [~,~,~,~,~,~,~,~,s] = rowerdynamics(q(i,:)',qd,qdd,xfw,par); clf plot(s(:,1),s(:,2),'o-'); hold on @@ -29,7 +29,11 @@ function animate(q, par) y = x*par.a + par.b; plot(x,y,'LineWidth',2); axis('equal'); - +% +% if i==37 +% pause +% end +% % Write each frame to the file if i==1 currFrame = getframe; diff --git a/model/dynfun.m b/model/dynfun.m index 3e641e6..0d7daa2 100644 --- a/model/dynfun.m +++ b/model/dynfun.m @@ -1,27 +1,30 @@ - function [f, dfdx, dfdxdot, dfdu,L , dLdq] = dynfun(x, xdot, u)%dLdx (x2 to 6 which are the angles) dLdq 1 by 5 matrix - % the 13 dynamics equations of the system, in the form + function [f, dfdx, dfdxdot, dfdu, L, dL_dq] = dynfun(x, xdot, u)%dLdx (x2 to 6 which are the angles) dLdq 1 by 5 matrix + % The 13 dynamics equations of the system, in the form % f(x, dx/dt, u) = 0 - % where x contains the 13 state variables and u are the 5 torques - % p contains the 5 model parameters + % where x contains the 13 state variables and u are the 5 joint torques + % This function also outputs the wrist-sprocket distance L and related Jacobian global model - % copy parameter values into variables with meaningful + % copy parameter values into variables with shorter names c = model.parameters.C; m = model.parameters.m ; K = model.parameters.K; % shock cord stiffness L0 = model.parameters.L0; % the wrist-sprocket distance when shock cord has zero force + + % Kcrm is probably no longer needed (remove it when confirmed) Kcrm = model.parameters.Kcrm; % chain rachet mechanism stiffness + Bcrm = model.parameters.Bcrm; % chain rachet mechanism damping - f = zeros(13,1); - - dfdx = spalloc(13,13,75); + % initialize the matrices + f = zeros(13,1); + dfdx = spalloc(13,13,74); % number of non-zero elements can be determint by using a breakpoint at the end of dynfun.m and then use nnz(dfdx) dfdxdot = spalloc(13,13,32); dfdu = spalloc(13,5,5); % x = [x_fw, q1 ...q5, v_fw, qd1...qd5, F] - % velocities are derivative of positions (2:01:00 recording 4) - f(1:6) = xdot(1:6) - x(7:12); %velocities + % velocities are derivative of positions + f(1:6) = xdot(1:6) - x(7:12); dfdx(1:6, 7:12) = -speye(6); dfdxdot(1:6, 1:6) = speye(6); @@ -29,32 +32,20 @@ q = x(2:6); % the five coordinates of the human model qd = x(8:12); qdd = xdot(8:12); - % rowerdynamics function generated by Autolev computes 5 torques - % using inverse dynamics and also 1 cable force using the - % cable ratchet mechanism model (z is a 6x1 output) - % 13 equations to model the dynamics. The - % first 6 (multibody dynamics and cable force) - [z, dz_dq, dz_dqd, dz_dqdd, dz_dFc] = rowerdynamics(q,qd,qdd,x(13),model.parameters);% inverse dynamics so gives us the z(torques) - % z has 6 elements, the first 5s are the torques, the 6th one is the - % wrist to sprocket distance, which depends on the q which is x(2:6) and - - %that we need to produce this motion with that force being applied to the - %wrist and then we compare those torques (motion trajectory) to the torques that we have in - %our trajectory (torque trajectory) and they have to be equal. + % rowerdynamics function generated by Autolev computes 5 torques + % using inverse dynamics and also the wrist-sprocket length L + [z, dz_dq, dz_dqd, dz_dqdd, dz_dFc, L, dL_dq, dLdot_dq, stick] = rowerdynamics(q,qd,qdd,x(13),model.parameters);% inverse dynamics so gives us the z(torques) + Ldot = dL_dq * qd; - f(8:12) = z(1:5) - u; % f is in Nm - dfdx(8:12, 2:6) = dz_dq(1:5,1:5); - dfdx(8:12, 8:12) = dz_dqd(1:5,1:5); - dfdx(8:12, 13) = dz_dFc(1:5); - dfdxdot(8:12, 8:12) = dz_dqdd(1:5,1:5); + % multibody dynamics + f(8:12) = z - u; + dfdx(8:12, 2:6) = dz_dq; + dfdx(8:12, 8:12) = dz_dqd; + dfdx(8:12, 13) = dz_dFc; + dfdxdot(8:12, 8:12) = dz_dqdd; dfdu(8:12, :) = -speye(5); - % flywheel dynamics, flywheel sees cable force, minus the shock cord - % force - L = z(6); - dLdq = dz_dq(6,:); - % dynamics of the flywheel % m*a - Fdamping - Fcrm = 0 % or m*a - Fdamping - (Ftotal - Fshockcord) = 0 @@ -62,18 +53,27 @@ dfdx(7,7) = 2*c*abs(x(7)); dfdx(7,13) = -1; dfdxdot(7,7) = m; - dfdx(7, 2:6) = K*dLdq; + dfdx(7, 2:6) = K*dL_dq; + + % ratchet mechanism force is a function of the wrist-sprocket distance + % L and flywheel position x(1) + + % start with a linear function + F = Kcrm * (L - x(1)) + Bcrm * (Ldot - x(7)); + + % a nonlinear transformation to change negative forces to zero + epsilon = 0.01; +% epsilon = 0.0001; + Fcrm = (F + sqrt(F^2 + epsilon^2) )/2; + dFcrm_dF = (1 + F/sqrt(F^2 + epsilon^2))/2; - %cable force must be equal to cable ratchet force + shock cord force - %use epsilon for non-linear spring to remove the non-linearity in the - %derivatives - epsilon = 0.000001; - Fsc= Kcrm * (L - x(1) ); - Fcrm = (Fsc + sqrt(Fsc^2+epsilon^2))/2 ; - dFcrmdFsc = (1+Fsc/sqrt(Fsc^2+epsilon^2))/2; + % this says that the force on the wrist is the sum of flywheel force + % and shock cord f(13) = x(13) - Fcrm - K*(L - L0); dfdx(13,13) = 1; - dfdx(13,1) = Kcrm*dFcrmdFsc; - dfdx(13,2:6) = -(dFcrmdFsc*Kcrm + K)*dLdq; + dfdx(13,1) = Kcrm * dFcrm_dF; + dfdx(13,7) = Bcrm * dFcrm_dF; + dfdx(13,2:6) = -(dFcrm_dF * Kcrm + K) * dL_dq - dFcrm_dF * Bcrm * dLdot_dq; + dfdx(13,8:12) = -dFcrm_dF * Bcrm * dL_dq; % because dLdot/dqdot = dL/dq end \ No newline at end of file diff --git a/model/makerowerdynamics.m b/model/makerowerdynamics.m index 48bd8bc..0fa1645 100644 --- a/model/makerowerdynamics.m +++ b/model/makerowerdynamics.m @@ -33,13 +33,13 @@ % write function header fprintf(fid2,'%% This file was generated by makerowerdynamics.m on %s\n', datetime); - fprintf(fid2,'function [f, df_dq, df_dqd, df_dqdd, df_dxfw, stick] = rowerdynamics(q,qd,qdd,xfw,par);\n'); + fprintf(fid2,'function [f, df_dq, df_dqd, df_dqdd, df_dFc, L, dL_dq, dLdot_dq, stick] = rowerdynamics(q,qd,qdd,Fc,par);\n'); fprintf(fid2,'%%\n'); fprintf(fid2,'%% Input:\n'); fprintf(fid2,'%% q...................(5x1) generalized coordinates (rad)\n'); fprintf(fid2,'%% qd..................(5x1) generalized velocities (rad/s)\n'); fprintf(fid2,'%% qdd.................(5x1) generalized accelerations (rad/s^2)\n'); - fprintf(fid2,'%% xfw.................(scalar) flywheel position (m)\n'); + fprintf(fid2,'%% Fc..................(scalar) cable force (N)\n'); fprintf(fid2,'%% par.................(structure) model parameters:\n'); fprintf(fid2,'%% par.TrunkMass, par.TrunkInertia, par.TrunkCM, par.TrunkLen\n'); fprintf(fid2,'%% par.ThighMass, par.ThighInertia, par.ThighCM, par.ThighLen\n'); @@ -49,15 +49,17 @@ fprintf(fid2,'%% par.Xankle, par.Yankle\n'); fprintf(fid2,'%% par.Xsprocket, par.Ysprocket %% point where the cable comes out of the machine\n'); fprintf(fid2,'%% par.a, par.b %% seat path is Y = a*X + b\n'); - fprintf(fid2,'%% par.K, par.C %% seat stiffness and damping\n'); - fprintf(fid2,'%% par.Kcrm %% cable stiffness parameter \n'); + fprintf(fid2,'%% par__Kseat, par__Cseat %% seat stiffness and damping\n'); fprintf(fid2,'%%\n'); fprintf(fid2,'%% Output:\n'); - fprintf(fid2,'%% f...................(6x1) 5 joint torques (Nm) and cable force (N)\n'); - fprintf(fid2,'%% df_dq...............(6x5) Jacobian matrix df/dq\n'); - fprintf(fid2,'%% df_dqd..............(6x5) Jacobian matrix df/dqdot\n'); - fprintf(fid2,'%% df_dqdd.............(6x5) Jacobian matrix df/dqdotdot\n'); - fprintf(fid2,'%% df_dxfw.............(6x1) Jacobian matrix df/dxfw\n'); + fprintf(fid2,'%% f...................(5x1) 5 joint torques\n'); + fprintf(fid2,'%% df_dq...............(5x5) Jacobian matrix df/dq\n'); + fprintf(fid2,'%% df_dqd..............(5x5) Jacobian matrix df/dqdot\n'); + fprintf(fid2,'%% df_dqdd.............(5x5) Jacobian matrix df/dqdotdot\n'); + fprintf(fid2,'%% df_dFc..............(5x1) Jacobian matrix df/dFc\n'); + fprintf(fid2,'%% L...................(scalar) wrist-sprocket distance (m)\n'); + fprintf(fid2,'%% dL_dq...............(1x5) Jacobian matrix dL/dq\n'); + fprintf(fid2,'%% dLdot_dq............(1x5) Jacobian matrix dLdot/dq\n'); fprintf(fid2,'%% stick...............(7x2) x,y of ankle,knee,hip,shoulder,elbow,wrist,sprocket\n'); fprintf(fid2,'%%\n'); @@ -68,8 +70,8 @@ fprintf(fid2,' q%1dpp = qdd(%1d);\n', i,i); end - % make sure that f comes out as a 6x1 column vector - fprintf(fid2,' f = zeros(6,1); \n'); + % make sure that f comes out as a 5x1 column vector + fprintf(fid2,' f = zeros(5,1); \n'); % copy the necessary parts of C code from fid1 to fid2 copying = 0; @@ -97,7 +99,7 @@ fprintf(fid2,' df_dq = sparse(df_dq);\n'); fprintf(fid2,' df_dqd = sparse(df_dqd);\n'); fprintf(fid2,' df_dqdd = sparse(df_dqdd);\n'); - fprintf(fid2,' df_dxfw = sparse(df_dxfw);\n'); + fprintf(fid2,' df_dFc = transpose(sparse(df_dFc));\n'); fprintf(fid2,'\nend \n'); fclose(fid2); diff --git a/model/mytest.m b/model/mytest.m index cd63d5d..a3bcd92 100644 --- a/model/mytest.m +++ b/model/mytest.m @@ -1,11 +1,104 @@ - function mytest -% runs a forward dynamic simulation of the rower model to test the dynamics code - global model +function mytest + % runs a forward dynamic simulation of the rower model to test the dynamics code + global model + tic + + addpath('../tools'); [~,~,~, model.parameters] = datainterp2; + + % test the derivatives of dynfun.m + disp('Testing dynfun.m'); + x = rand(13,1); + xd = rand(13,1); + u = rand(5,1); + [f, df_dx, df_dxd, df_du, L, dL_dq] = dynfun(x, xd, u); + df_dx_num = zeros(13,13); + df_dxd_num = zeros(13,13); + df_du_num = zeros(13,5); + dL_dq_num = zeros(1,5); + hh = 1e-7; + for i = 1:13 + save = x(i); + x(i) = x(i) + hh; + [fh,~,~,~,Lh] = dynfun(x, xd, u); + df_dx_num(:,i) = (fh - f)/hh; + if (i>=2) && (i<=6) + dL_dq_num(1,i-1) = (Lh - L)/hh; + end + x(i) = save; + %--------------------- + save = xd(i); + xd(i) = xd(i) + hh; + fh = dynfun(x, xd, u); + df_dxd_num(:,i) = (fh - f)/hh; + xd(i) = save; + end + for i = 1:5 + save = u(i); + u(i) = u(i) + hh; + fh = dynfun(x, xd, u); + df_du_num(:,i) = (fh - f)/hh; + u(i) = save; + end + fprintf('Max. error in df_dx: '); matcompare(df_dx, df_dx_num); + fprintf('Max. error in df_dxd: '); matcompare(df_dxd, df_dxd_num); + fprintf('Max. error in df_du: '); matcompare(df_du, df_du_num); + fprintf('Max. error in dL_dq: '); matcompare(dL_dq, dL_dq_num); + + + % test the derivatives of rowerdynamics.m + disp('Testing rowerdynamics.m...'); + q = rand(5,1); + qd = rand(5,1); + qdd = rand(5,1); + Fc = rand(1,1); + [f, df_dq, df_dqd, df_dqdd, df_dFc, L, dL_dq, dLdot_dq] = rowerdynamics(q,qd,qdd,Fc,model.parameters); + df_dq_num = zeros(5,5); + df_dqd_num = zeros(5,5); + df_dqdd_num = zeros(5,5); + df_dFc_num = zeros(5,1); + dL_dq_num = zeros(1,5); + dLdot_dq_num = zeros(1,5); + Ldot = dL_dq * qd; + hh = 1e-7; + for i = 1:5 + save = q(i); + q(i) = q(i) + hh; + [fh,~,~,~,~,Lh,dL_dqh] = rowerdynamics(q,qd,qdd,Fc,model.parameters); + df_dq_num(:,i) = (fh - f)/hh; + dL_dq_num(:,i) = (Lh - L)/hh; + Ldoth = dL_dqh * qd; + dLdot_dq_num(:,i) = (Ldoth - Ldot)/hh; + q(i) = save; + %----------------------- + save = qd(i); + qd(i) = qd(i) + hh; + fh = rowerdynamics(q,qd,qdd,Fc,model.parameters); + df_dqd_num(:,i) = (fh - f)/hh; + qd(i) = save; + %----------------------- + save = qdd(i); + qdd(i) = qdd(i) + hh; + fh = rowerdynamics(q,qd,qdd,Fc,model.parameters); + df_dqdd_num(:,i) = (fh - f)/hh; + qdd(i) = save; + end + save = Fc; + Fc = Fc + hh; + fh = rowerdynamics(q,qd,qdd,Fc,model.parameters); + df_dFc_num(:,1) = (fh - f)/hh; + Fc = save; + fprintf('Max. error in df_dq: '); matcompare(df_dq, df_dq_num); + fprintf('Max. error in df_dqd: '); matcompare(df_dqd, df_dqd_num); + fprintf('Max. error in df_dqdd: '); matcompare(df_dqdd, df_dqdd_num); + fprintf('Max. error in df_dFc: '); matcompare(df_dFc, df_dFc_num); + fprintf('Max. error in dL_dq: '); matcompare(dL_dq, dL_dq_num); + fprintf('Max. error in dLdot_dq: '); matcompare(dLdot_dq, dLdot_dq_num); + keyboard - % choose a suitable initial condition: realistic joint angles, and choose flywheel position y(1) - % to be equal to the L that comes from these joint angles - % and make all velocities zero + % choose a suitable initial condition: realistic joint angles, and choose flywheel position y(1) + % to be equal to the L that comes from these joint angles + % and make all velocities zero y0 = [ 0.1 ;[86.6804 ; 121.4658 ; 60.3518 ; 61.3895 ; 12.4060] * pi/180; 0 ; 0; 0; 0; 0;0; 0]; % initial condition () yp0 = zeros(13,1); % initial guess for state derivatives at t=0 %fixed_y0 = [0 1 1 1 1 1 1 0 0 0 0 0 0]; @@ -14,7 +107,7 @@ fixed_yp0 = []; T = 0:0.01:2.0; % simulation time [y0,yp0] = decic(@dyn,T(1),y0,fixed_y0,yp0,fixed_yp0); % find consistent initial conditions - [tt,yy,L] = ode15i(@dyn, T, y0,yp0); % simulate using ode15i + [tt,yy] = ode15i(@dyn, T, y0,yp0); % simulate using ode15i % make the AVI file disp('making AVI file...') @@ -24,79 +117,75 @@ figure(1); animate(q,model.parameters); - % make plots + % make plots figure(2) plot(tt, q*180/pi, 'LineWidth',2); - legend('q1','q2','q3','q4','q5','f'); + legend('q1','q2','q3','q4','q5'); xlabel('time (s)','fontweight','bold','fontsize',10); ylabel('angle (deg)','fontweight','bold','fontsize',10); title('Joint Angles','fontweight','bold','fontsize',10) figure(3) plot(tt, f, 'LineWidth',2); xlabel('time (s)','fontweight','bold','fontsize',10); - ylabel('force (N)','fontweight','bold','fontsize',10); + ylabel('wrist force (N)','fontweight','bold','fontsize',10); title('Force','fontweight','bold','fontsize',10) figure(4) plot(tt, yy(:,1), 'LineWidth',2); xlabel('time (s)','fontweight','bold','fontsize',10); ylabel('fwpos (m)','fontweight','bold','fontsize',10); - title('fwpos','fontweight','bold','fontsize',10) + title('flywheel position','fontweight','bold','fontsize',10) figure(5) plot(tt, yy(:,7), 'LineWidth',2); xlabel('time (s)','fontweight','bold','fontsize',10); - ylabel('fwvelo (m)','fontweight','bold','fontsize',10); - title('fwvelo','fontweight','bold','fontsize',10) - + ylabel('fwvelo (m/s)','fontweight','bold','fontsize',10); + title('flywheel velocity','fontweight','bold','fontsize',10) end %======================================================================= function f = dyn(t,y, ydot) -% dynfun(t,y,ydot) -% dynamics of the rower model, in implicit form f(y,ydot,t) = 0 -% where y is (q,qdot) - global model - f = zeros(10,1); % f will be a 10x1 matrix - - % extract q, qdot, qdotdot from y and ydot - q = y(2:6); - qd = y(8:12); + % extract q, qdot from y + q = y(2:6); + qd = y(8:12); - % generate joint torques with a PD controller + % generate joint torques with a PD controller % controller is turned on at tstart - % simulation 1: tstart = 10, model should just fall down - % simulation 2, use initial angles as the setpoint, and tstart = 0 - % simulation 3, use a leaning back setpoint, to make the human pull - % simulation 4, use tstart = 0.5, model should fall down and come back up - qsetpoint = [152.5304 ; 18.1593; 114.5404 ; -23.0064; 102.3893] * pi/180; - + % simulation 1: tstart = 10, model should just fall down + % simulation 2, use initial angles as the setpoint, and tstart = 0 + % simulation 3, use a leaning back setpoint, to make the human pull + % simulation 4, use tstart = 0.5, model should fall down and come back up + qsetpoint = [152.5304 ; 18.1593; 114.5404 ; -23.0064; 102.3893] * pi/180; + tstart = 0.4; + if t < tstart + kp = 0; + kd = 0; + else + kp = 600; + kd = 50; + end + tau = -kp*(q-qsetpoint) - kd*qd; - tstart = 0.4; - if t < tstart - kp = 0; - kd = 0; - else - kp = 600; - kd = 50; - end - tau = -kp*(q-qsetpoint) - kd*qd; - [f,~,~,~,L] = dynfun(y,ydot,tau); -% % equations 1-5 of the implicit dynamics: derivative of q must be equal to qdot -% f(1:5) = ydot(1:5) - y(6:10); -% -% % equations 6-10: inverse dynamic torque must be equal to the applied torque -% xfw = 100; % move the flywheel far to the right, so no cable force will be generated -% ff = rowerdynamics(q,qd,qdd,xfw,model.parameters); % do the inverse dynamic calculation -% f(6:10) = tau - ff(1:5); % first 5 elements of ff are the inverse dynamic torques + % evaluate the implicit dynamics function + f = dynfun(y,ydot,tau); -% if toc > 1.0 -% fprintf('simulating... t = %8.3f\n',t); -% tic -% end + if toc > 1.0 + fprintf('simulating... t = %8.3f\n',t); + tic + end - - end -%======================================================================= \ No newline at end of file +%==================================================================== + function matcompare(a,b) + % compares two matrices and prints element that has greatest difference + if (size(a,1) == 1) + irow = 1; + [maxerr,icol] = max(abs(a-b)); + else + [maxerr,irow] = max(abs(a-b)); + [maxerr,icol] = max(maxerr); + irow = irow(icol); + end + fprintf('%9.6f at %d %d (%9.6f vs. %9.6f)\n', full(maxerr), irow, icol, full(a(irow,icol)), full(b(irow,icol))); + end diff --git a/model/rowerdynamics.al b/model/rowerdynamics.al index 057c5ad..df02640 100644 --- a/model/rowerdynamics.al +++ b/model/rowerdynamics.al @@ -2,8 +2,13 @@ % % Autolev code to compute inverse dynamics of the rower model, including the cable model % +% The matlab function makerowerdynamics.m will transform this into a Matlab function that +% looks like this: +% +% function [f, df_dq, df_dqd, df_dqdd, df_dFc, stick] = rowerdynamics(q,qd,qdd,Fc,par) +% % Author: Ton van den Bogert -% Last revised: 12/21/2016 +% Last revised: 2/15/2018 PAUSE 0 AUTOZ ON @@ -36,10 +41,10 @@ Constants par__ForearmMass, par__ForearmInertia, par__ForearmCM, par__ForearmLen Constants par__Xankle, par__Yankle Constants par__Xsprocket, par__Ysprocket % point where the cable comes out of the machine Constants par__a, par__b % seat path is Y = a*X + b -Constants par__K, par__C % seat stiffness and damping -Constants par__Kcrm % cable stiffness parameter +Constants par__Kseat, par__Cseat % seat stiffness and damping +Constants par__Kcrm, par__K % cable stiffness and return spring stiffness Constants par__g % gravity -Constants xfw % flywheel position +Constants Fc % cable force (input from Matlab dynamics function) %---------------------------------- % Define bodies and points @@ -133,17 +138,17 @@ A2pts(Ground, Forearm, Elbow, ForearmO); gravity(-par__g*Ground2>) PP> = (par__a*Ground1> - Ground2>) / sqrt(1+par__a*par__a) % a unit vector that is perpendicular to the line d = dot(P_GroundO_Hip> - par__b*Ground2>,PP>) % perpendicular distance from Hip to the line -force_Hip> += -par__K*d*PP> - par__C*dt(d)*PP> % spring and damper force perpendicular to the line and +force_Hip> += -par__Kseat*d*PP> - par__Cseat*dt(d)*PP> % spring and damper force perpendicular to the line and %------------------------------------------------------- % compute wrist position and cable force %------------------------------------------------------- P_Elbow_Wrist> = par__ForearmLen*Forearm1> V2pts(Ground, Forearm, Elbow, Wrist); % velocity is needed for equations of motion -cable> = P_GroundO_Wrist> - Vector(Ground,par__Xsprocket,par__Ysprocket,0) % vector from sprocket to wrist -Lc = mag(cable>) - xfw; % distance between wrist and flywheel position -Fc = par__K * (Lc + abs(Lc))/2 % cable force is always positive -force_Wrist> += -Fc * unitvec(cable>) % force vector acting on the wrist +cable> = Vector(Ground,par__Xsprocket,par__Ysprocket,0) - P_GroundO_Wrist> % vector from wrist to sprocket +L = mag(cable>); % distance between wrist and sprocket +Ldot = dt(L); +force_Wrist> += Fc * unitvec(cable>) % force vector acting on the wrist %---------------------------------------------------------- % x and y coordinates of the joints and sprocket, stored in a 6x2 matrix @@ -159,7 +164,7 @@ stick = [ par__Xankle, par__Yankle ; & %---------------------------------------------------------------- % generate the outputs %---------------------------------------------------------------- -f = [-fr()-frstar() ; Fc] % the five torques (Nm) and the cable force (N) +f = -fr()-frstar() % the five torques % vectors needed to define Jacobian matrices q = [q1, q2, q3, q4, q5] @@ -170,9 +175,11 @@ qdd = [q1'', q2'', q3'', q4'', q5''] df_dq = ZEE(D(f,q)) df_dqd = ZEE(D(f,qd)) df_dqdd = ZEE(D(f,qdd)) -df_dxfw = ZEE(D(f,xfw)) +df_dFc = ZEE(D(f,Fc)) +dL_dq = ZEE(D(L,q)) +dLdot_dq = ZEE(D(Ldot,q)) -Encode f, df_dq, df_dqd, df_dqdd, df_dxfw, stick +Encode f, df_dq, df_dqd, df_dqdd, df_dFc, L, dL_dq, dLdot_dq, stick Code Algebraic() tmp.m EXIT diff --git a/model/rowerdynamics.m b/model/rowerdynamics.m index 5b3480c..43f2452 100644 --- a/model/rowerdynamics.m +++ b/model/rowerdynamics.m @@ -1,5 +1,5 @@ -% This file was generated by makerowerdynamics.m on 31-May-2017 12:14:36 -function [f, df_dq, df_dqd, df_dqdd, df_dFc, stick] = rowerdynamics(q,qd,qdd,Fc,par); +% This file was generated by makerowerdynamics.m on 20-Feb-2018 16:03:12 +function [f, df_dq, df_dqd, df_dqdd, df_dFc, L, dL_dq, dLdot_dq, stick] = rowerdynamics(q,qd,qdd,Fc,par); % % Input: % q...................(5x1) generalized coordinates (rad) @@ -18,11 +18,14 @@ % par__Kseat, par__Cseat % seat stiffness and damping % % Output: -% f...................(6x1) 5 joint torques (Nm) and the wrist-sprocket distance (m) -% df_dq...............(6x5) Jacobian matrix df/dq -% df_dqd..............(6x5) Jacobian matrix df/dqdot -% df_dqdd.............(6x5) Jacobian matrix df/dqdotdot -% df_dFc.............(6x1) Jacobian matrix df/dFc +% f...................(5x1) 5 joint torques +% df_dq...............(5x5) Jacobian matrix df/dq +% df_dqd..............(5x5) Jacobian matrix df/dqdot +% df_dqdd.............(5x5) Jacobian matrix df/dqdotdot +% df_dFc..............(5x1) Jacobian matrix df/dFc +% L...................(scalar) wrist-sprocket distance (m) +% dL_dq...............(1x5) Jacobian matrix dL/dq +% dLdot_dq............(1x5) Jacobian matrix dLdot/dq % stick...............(7x2) x,y of ankle,knee,hip,shoulder,elbow,wrist,sprocket % q1 = q(1); @@ -40,14 +43,14 @@ q5 = q(5); q5p = qd(5); q5pp = qdd(5); - f = zeros(6,1); + f = zeros(5,1); % Unit conversions Pi = 3.141592653589793; DEGtoRAD = Pi/180.0; RADtoDEG = 180.0/Pi; % Reserve space and initialize matrices - z = zeros(570,1); + z = zeros(678,1); % Evaluate constants z(1) = cos(q1); @@ -175,363 +178,363 @@ z(132) = z(7)*z(128) + z(8)*z(130); z(133) = z(7)*z(129) + z(8)*z(131); L = (par.ForearmLen^2+par.ShankLen^2+par.ThighLen^2+par.TrunkLen^2+par.UpparmLen^2+z(111)^2+z(112)^2+2*par.ForearmLen*par.ShankLen*z(113)+2*par.ForearmLen*par.ThighLen*z(117)+2*par.ForearmLen*par.UpparmLen*z(72)+2*par.ShankLen*par.ThighLen*z(7)+2*par.ShankLen*par.UpparmLen*z(128)+2*par.ThighLen*par.UpparmLen*z(132)+2*par.TrunkLen*z(111)*z(45)+2*par.TrunkLen*z(112)*z(47)-2*par.ForearmLen*par.TrunkLen*z(121)-2*par.ForearmLen*z(111)*z(74)-2*par.ForearmLen*z(112)*z(76)-2*par.ShankLen*par.TrunkLen*z(124)-2*par.ShankLen*z(111)*z(1)-2*par.ShankLen*z(112)*z(2)-2*par.ThighLen*par.TrunkLen*z(23)-2*par.ThighLen*z(111)*z(9)-2*par.ThighLen*z(112)*z(11)-2*par.TrunkLen*par.UpparmLen*z(43)-2*par.UpparmLen*z(111)*z(49)-2*par.UpparmLen*z(112)*z(51))^0.5; - z(136) = par.ForearmLen^2 + par.ShankLen^2 + par.ThighLen^2 + par.TrunkLen^2 + par.UpparmLen^2 + z(111)^2 + z(112)^2; - z(137) = par.ForearmLen*par.ShankLen; - z(138) = par.ForearmLen*par.ThighLen; - z(139) = par.ForearmLen*par.UpparmLen; - z(140) = par.ShankLen*par.ThighLen; - z(141) = par.ShankLen*par.UpparmLen; - z(142) = par.ThighLen*par.UpparmLen; - z(143) = par.TrunkLen*z(111); - z(144) = par.TrunkLen*z(112); - z(145) = par.ForearmLen*par.TrunkLen; - z(146) = par.ForearmLen*z(111); - z(147) = par.ForearmLen*z(112); - z(148) = par.ShankLen*par.TrunkLen; - z(149) = par.ShankLen*z(111); - z(150) = par.ShankLen*z(112); - z(151) = par.ThighLen*par.TrunkLen; - z(152) = par.ThighLen*z(111); - z(153) = par.ThighLen*z(112); - z(154) = par.TrunkLen*par.UpparmLen; - z(155) = par.UpparmLen*z(111); - z(156) = par.UpparmLen*z(112); - z(157) = z(136) + 2*z(137)*z(113) + 2*z(138)*z(117) + 2*z(139)*z(72) + 2*z(140)*z(7) + 2*z(141)*z(128) + 2*z(142)*z(132) + 2*z(143)*z(45) + 2*z(144)*z(47) - 2*z(145)*z(121) - 2*z(146)*z(74) - 2*z(147)*z(76) - 2*z(148)*z(124) - 2*z(149)*z(1) - 2*z(150)*z(2) - 2*z(151)*z(23) - 2*z(152)*z(9) - 2*z(153)*z(11) - 2*z(154)*z(43) - 2*z(155)*z(49) - 2*z(156)*z(51); - z(158) = z(157)^0.5; - z(159) = par.ForearmLen/z(158); - z(160) = z(111)/z(158); - z(161) = z(112)/z(158); - z(162) = par.ShankLen/z(158); - z(163) = par.ThighLen/z(158); - z(164) = par.TrunkLen/z(158); - z(165) = par.UpparmLen/z(158); - z(166) = Fc*z(159); - z(167) = Fc*z(160); - z(168) = Fc*z(161); - z(169) = Fc*z(162); - z(170) = Fc*z(163); - z(171) = Fc*z(164); - z(172) = Fc*z(165); - z(173) = par.ShankCM*z(96); - z(174) = z(73)*z(108)*z(172) + z(74)*z(78)*z(167) + z(75)*z(108)*z(167) + z(76)*z(78)*z(168) + z(77)*z(108)*z(168) + z(78)*z(121)*z(171) + z(108)*z(123)*z(171) + z(9)*z(20)*z(106) + z(11)*z(12)*z(106) - z(173)*z(1) - z(78)*z(166) - z(72)*z(78)*z(172) - z(78)*z(113)*z(169) - z(78)*z(117)*z(170) - z(108)*z(115)*z(169) - z(108)*z(119)*z(170) - z(95)*(z(76)*z(78)+z(77)*z(87)) - z(97)*(z(9)*z(14)+z(11)*z(12)) - z(98)*(z(25)*z(47)+z(32)*z(48)) - z(99)*(z(51)*z(53)+z(52)*z(59)) - z(9)*z(12)*z(105) - z(10)*z(20)*z(105); - z(175) = par.ThighCM*z(97); - z(176) = z(73)*z(109)*z(172) + z(74)*z(79)*z(167) + z(75)*z(109)*z(167) + z(76)*z(79)*z(168) + z(77)*z(109)*z(168) + z(79)*z(121)*z(171) + z(109)*z(123)*z(171) + par.ThighLen*(z(9)*z(106)-z(10)*z(105)) - z(175)*z(9) - z(79)*z(166) - z(72)*z(79)*z(172) - z(79)*z(113)*z(169) - z(79)*z(117)*z(170) - z(109)*z(115)*z(169) - z(109)*z(119)*z(170) - z(95)*(z(76)*z(79)+z(77)*z(88)) - z(98)*(z(26)*z(47)+z(31)*z(48)) - z(99)*(z(51)*z(54)+z(52)*z(60)); - z(177) = z(98)*z(30)*z(48) + z(73)*z(110)*z(172) + z(74)*z(80)*z(167) + z(75)*z(110)*z(167) + z(76)*z(80)*z(168) + z(77)*z(110)*z(168) + z(80)*z(121)*z(171) + z(110)*z(123)*z(171) + z(99)*(z(51)*z(55)-z(52)*z(61)) - z(80)*z(166) - z(72)*z(80)*z(172) - z(80)*z(113)*z(169) - z(80)*z(117)*z(170) - z(110)*z(115)*z(169) - z(110)*z(119)*z(170) - z(95)*(z(76)*z(80)+z(77)*z(89)); - z(178) = par.UpparmCM*z(99); - z(179) = z(73)*z(107)*z(172) + z(74)*z(81)*z(167) + z(75)*z(107)*z(167) + z(76)*z(81)*z(168) + z(77)*z(107)*z(168) + z(81)*z(121)*z(171) + z(107)*z(123)*z(171) - z(178)*z(52) - z(81)*z(166) - z(72)*z(81)*z(172) - z(81)*z(113)*z(169) - z(81)*z(117)*z(170) - z(107)*z(115)*z(169) - z(107)*z(119)*z(170) - z(95)*(z(76)*z(81)+z(77)*z(86)); - z(180) = par.ForearmCM*z(95); - z(181) = -z(180)*z(77) - par.ForearmLen*(z(115)*z(169)+z(119)*z(170)-z(73)*z(172)-z(75)*z(167)-z(77)*z(168)-z(123)*z(171)); - z(182) = z(2)*z(8) - z(1)*z(7); - z(183) = z(10)*z(23) + z(24)*z(182); - z(184) = z(23)*z(182) - z(10)*z(24); - z(185) = -z(9)*z(23) - z(10)*z(24); - z(186) = -z(9)*z(24) - z(11)*z(23); - z(187) = z(45)*z(48) + z(46)*z(183); - z(188) = z(46)^2 + z(48)^2; - z(190) = -z(1)*z(8)*q1p - z(1)*z(8)*q2p - z(2)*z(7)*q1p - z(2)*z(7)*q2p; - z(191) = z(2)*z(8)*q1p + z(2)*z(8)*q2p - z(1)*z(7)*q1p - z(1)*z(7)*q2p; - z(192) = z(10)*z(23)*q3p + z(23)*z(190) + z(24)*z(191) - z(9)*z(24)*q3p; - z(193) = z(1)*z(7)*q1p + z(1)*z(7)*q2p - z(2)*z(8)*q1p - z(2)*z(8)*q2p; - z(194) = z(23)*z(190) - z(9)*z(24)*q3p - z(11)*z(23)*q3p - z(24)*z(193); - z(195) = z(23)*z(191) - z(9)*z(23)*q3p - z(10)*z(24)*q3p - z(24)*z(190); - z(196) = z(1)*z(8)*q1p + z(1)*z(8)*q2p + z(2)*z(7)*q1p + z(2)*z(7)*q2p; - z(197) = z(23)*z(182)*q3p + z(23)*z(191) + z(24)*z(196) - z(10)*z(24)*q3p; - z(198) = z(45)*q1p*z(194) + z(45)*q2p*z(194) + z(46)*q1p*z(197) + z(46)*q2p*z(197) + z(48)*q1p*z(192) + z(48)*q2p*z(192) + z(183)*q1p*z(195) + z(183)*q2p*z(195) + 2*z(46)*q3p*z(195) + 2*z(48)*q3p*z(194); - z(199) = par.TrunkInertia*z(187); - z(200) = par.TrunkInertia*z(188); - z(201) = par.TrunkInertia*z(198); - z(202) = par.ForearmInertia + par.ShankInertia + par.ThighInertia + par.UpparmInertia + par.ShankMass*par.ShankCM^2; - z(203) = z(202) + z(187)*z(199) + par.ForearmMass*(z(78)^2+z(87)^2) + par.ThighMass*(z(12)^2+z(14)^2) + par.TrunkMass*(z(25)^2+z(32)^2) + par.UpparmMass*(z(53)^2+z(59)^2); - z(204) = par.ForearmInertia + par.ThighInertia + par.UpparmInertia; - z(205) = par.ThighCM*par.ThighMass; - z(206) = z(204) + z(205)*z(14) + z(187)*z(199) + par.ForearmMass*(z(78)*z(79)+z(87)*z(88)) + par.TrunkMass*(z(25)*z(26)+z(31)*z(32)) + par.UpparmMass*(z(53)*z(54)+z(59)*z(60)); - z(207) = z(204) + z(187)*z(200) + par.ForearmMass*(z(78)*z(80)+z(87)*z(89)) - par.TrunkMass*z(30)*z(32) - par.UpparmMass*(z(53)*z(55)-z(59)*z(61)); - z(208) = par.ForearmInertia + par.UpparmInertia; - z(209) = par.UpparmCM*par.UpparmMass; - z(210) = z(208) + z(209)*z(59) + par.ForearmMass*(z(78)*z(81)+z(86)*z(87)); - z(211) = par.ForearmCM*par.ForearmMass; - z(212) = par.ForearmInertia + z(211)*z(87); - z(213) = z(187)*z(201) + par.ForearmMass*(z(78)*z(94)+z(87)*z(91)) + par.ThighMass*(z(12)*z(19)+z(14)*z(16)) + par.TrunkMass*(z(25)*z(38)+z(32)*z(34)) + par.UpparmMass*(z(53)*z(66)+z(59)*z(63)); - z(214) = par.ForearmInertia + par.ThighInertia + par.UpparmInertia + par.ThighMass*par.ThighCM^2; - z(215) = z(214) + z(187)*z(199) + par.ForearmMass*(z(79)^2+z(88)^2) + par.TrunkMass*(z(26)^2+z(31)^2) + par.UpparmMass*(z(54)^2+z(60)^2); - z(216) = z(204) + z(187)*z(200) + par.ForearmMass*(z(79)*z(80)+z(88)*z(89)) - par.TrunkMass*z(30)*z(31) - par.UpparmMass*(z(54)*z(55)-z(60)*z(61)); - z(217) = z(208) + z(209)*z(60) + par.ForearmMass*(z(79)*z(81)+z(86)*z(88)); - z(218) = par.ForearmInertia + z(211)*z(88); - z(219) = z(205)*z(16) + z(187)*z(201) + par.ForearmMass*(z(79)*z(94)+z(88)*z(91)) + par.TrunkMass*(z(26)*z(38)+z(31)*z(34)) + par.UpparmMass*(z(54)*z(66)+z(60)*z(63)); - z(220) = z(204) + z(188)*z(199) + par.ForearmMass*(z(78)*z(80)+z(87)*z(89)) - par.TrunkMass*z(30)*z(32) - par.UpparmMass*(z(53)*z(55)-z(59)*z(61)); - z(221) = z(204) + z(188)*z(199) + par.ForearmMass*(z(79)*z(80)+z(88)*z(89)) - par.TrunkMass*z(30)*z(31) - par.UpparmMass*(z(54)*z(55)-z(60)*z(61)); - z(222) = z(204) + z(188)*z(200) + par.TrunkMass*z(30)^2 + par.ForearmMass*(z(80)^2+z(89)^2) + par.UpparmMass*(z(55)^2+z(61)^2); - z(223) = z(208) + z(209)*z(61) + par.ForearmMass*(z(80)*z(81)+z(86)*z(89)); - z(224) = par.ForearmInertia + z(211)*z(89); - z(225) = z(188)*z(201) + par.ForearmMass*(z(80)*z(94)+z(89)*z(91)) - par.TrunkMass*z(30)*z(34) - par.UpparmMass*(z(55)*z(66)-z(61)*z(63)); - z(226) = par.ForearmInertia + par.UpparmInertia + par.UpparmMass*par.UpparmCM^2; - z(227) = z(226) + par.ForearmMass*(z(81)^2+z(86)^2); - z(228) = par.ForearmInertia + z(211)*z(86); - z(229) = z(209)*z(63) + par.ForearmMass*(z(81)*z(94)+z(86)*z(91)); - z(230) = par.ForearmInertia + par.ForearmMass*par.ForearmCM^2; - z(231) = z(211)*z(91); - z(232) = z(11)*z(24) + z(23)*z(182); - z(233) = z(45)*z(46) + z(46)*z(232) + z(48)*z(183) + z(183)*z(184); - z(234) = z(11)*z(23)*q3p + z(23)*z(196) + z(24)*z(193) - z(24)*z(182)*q3p; - z(235) = z(23)*z(196) - z(10)*z(23)*q3p - z(24)*z(182)*q3p - z(24)*z(191); - z(236) = z(45)*q1p*z(195) + z(45)*q2p*z(195) + z(46)*q1p*z(192) + z(46)*q1p*z(234) + z(46)*q2p*z(192) + z(46)*q2p*z(234) + z(48)*q1p*z(197) + z(48)*q2p*z(197) + z(183)*q1p*z(194) + z(183)*q1p*z(235) + z(183)*q2p*z(194) + z(183)*q2p*z(235) + z(184)*q1p*z(197) + z(184)*q2p*z(197) + z(232)*q1p*z(195) + z(232)*q2p*z(195) + 2*z(46)*q3p*z(194) + 2*z(46)*q3p*z(235) + 2*z(48)*q3p*z(195) + 2*z(184)*q3p*z(195); - z(237) = z(233)*z(201) + par.TrunkInertia*z(187)*z(236); - z(238) = z(233)*(z(199)+par.TrunkInertia*z(187)); - z(239) = par.TrunkInertia*z(46)*(z(48)+z(184)); - z(240) = z(200)*z(233) + 2*z(187)*z(239); - z(241) = z(43)*z(183) + z(44)*z(184); - z(242) = z(43)*z(184) - z(44)*z(183); - z(243) = z(72)*z(241) + z(73)*z(242); - z(244) = z(1)*(z(76)+z(243)); - z(245) = z(9)*z(76) + z(9)*z(243) + z(10)*z(74) + z(11)*z(74); - z(246) = z(1)*(z(51)+z(241)); - z(247) = z(2)*(z(51)+z(241)); - z(248) = z(7)*z(246) - z(8)*z(247); - z(249) = z(1)*(z(47)+z(183)); - z(250) = 2*z(137)*z(244) + 2*z(138)*z(245) + 2*z(141)*z(246) + 2*z(142)*z(248) + 2*z(143)*z(183) + 2*z(144)*z(45) + 2*z(149)*z(2) - 2*z(146)*z(243) - 2*z(147)*z(74) - 2*z(148)*z(249) - 2*z(150)*z(1) - 2*z(152)*z(10) - 2*z(153)*z(9) - 2*z(155)*z(241) - 2*z(156)*z(49); - z(251) = par.UpparmLen*z(250)/(z(157)^0.5*z(158)^2); - z(252) = z(111)*z(250)/(z(157)^0.5*z(158)^2); - z(253) = z(72)*z(242) - z(73)*z(241); - z(254) = z(112)*z(250)/(z(157)^0.5*z(158)^2); - z(255) = par.TrunkLen*z(250)/(z(157)^0.5*z(158)^2); - z(256) = (par.ThighLen*z(10)+z(100)*z(1)-par.ShankLen*z(2)-z(101)*z(182))/z(102)^0.5; - z(257) = par.ThighLen*(z(10)-par.a*z(182))/z(102)^0.5; - z(258) = (par.Kseat*z(103)+par.Cseat*(z(256)*q1p+z(257)*q2p))/z(102)^0.5; - z(259) = par.ForearmLen*z(250)/(z(157)^0.5*z(158)^2); - z(260) = par.ShankLen*z(250)/(z(157)^0.5*z(158)^2); - z(261) = par.ThighLen*z(250)/(z(157)^0.5*z(158)^2); - z(262) = z(1)*(z(77)+z(253)); - z(263) = z(9)*z(77) + z(9)*z(253) + z(10)*z(75) + z(11)*z(75); - z(264) = par.a*(par.Kseat*z(103)+par.Cseat*(z(256)*q1p+z(257)*q2p))/z(102)^0.5; - z(265) = z(173)*z(2) + z(74)*z(78)*z(168) + z(75)*z(108)*z(168) + z(78)*z(167)*z(243) + z(108)*z(167)*z(253) + 0.5*Fc*z(78)*z(259) + 0.5*Fc*z(72)*z(78)*z(251) + 0.5*Fc*z(78)*z(113)*z(260) + 0.5*Fc*z(78)*z(117)*z(261) + 0.5*Fc*z(108)*z(115)*z(260) + 0.5*Fc*z(108)*z(119)*z(261) + z(9)*z(12)*z(106) + z(9)*z(12)*z(264) + z(10)*z(20)*z(106) + z(10)*z(20)*z(264) - z(78)*z(169)*z(244) - z(78)*z(170)*z(245) - z(108)*z(169)*z(262) - z(108)*z(170)*z(263) - 0.5*Fc*z(73)*z(108)*z(251) - 0.5*Fc*z(74)*z(78)*z(252) - 0.5*Fc*z(75)*z(108)*z(252) - 0.5*Fc*z(76)*z(78)*z(254) - 0.5*Fc*z(77)*z(108)*z(254) - 0.5*Fc*z(78)*z(121)*z(255) - 0.5*Fc*z(108)*z(123)*z(255) - z(95)*(z(74)*z(78)+z(75)*z(87)) - z(97)*(z(9)*z(12)+z(10)*z(14)) - z(98)*(z(25)*z(45)+z(32)*z(46)) - z(99)*(z(49)*z(53)+z(50)*z(59)) - z(9)*z(20)*z(258) - z(10)*z(12)*z(105) - z(11)*z(12)*z(258) - z(20)*z(182)*z(105); - z(266) = par.ShankLen*(z(7)*z(23)-z(8)*z(24)); - z(267) = par.ShankLen*(z(7)*z(24)+z(8)*z(23)); - z(268) = z(43)*z(266) - z(44)*z(267); - z(269) = -z(43)*z(267) - z(44)*z(266); - z(270) = z(72)*z(268) + z(73)*z(269); - z(271) = (z(7)*z(24)+z(8)*z(23))*z(6); - z(272) = (z(7)*z(23)-z(8)*z(24))*z(6); - z(273) = z(43)*z(271) + z(44)*z(272); - z(274) = z(43)*z(272) - z(44)*z(271); - z(275) = z(72)*z(273) + z(73)*z(274); - z(276) = z(72)*z(269) - z(73)*z(268); - z(277) = z(72)*z(274) - z(73)*z(273); - z(278) = z(233)*z(201) + par.TrunkInertia*z(187)*z(236) + par.ForearmMass*(z(78)*z(275)+z(87)*z(277)+z(270)*z(94)+z(276)*z(91)) + par.UpparmMass*(z(53)*z(273)+z(59)*z(274)+z(268)*z(66)+z(269)*z(63)) + par.TrunkMass*(z(25)*z(271)+z(32)*z(272)+z(266)*z(38)-z(267)*z(34)) - par.ThighMass*(par.ShankLen*z(8)*z(16)-par.ShankLen*z(7)*z(19)-z(7)*z(14)*z(6)-z(8)*z(12)*z(6)); - z(279) = par.ShankLen*par.ThighMass; - z(280) = z(199)*z(233) + par.TrunkInertia*z(187)*z(233) + 2*par.ForearmMass*(z(78)*z(270)+z(87)*z(276)) + 2*par.UpparmMass*(z(53)*z(268)+z(59)*z(269)) + 2*par.TrunkMass*(z(25)*z(266)-z(32)*z(267)) + 2*z(279)*(z(7)*z(12)-z(8)*z(14)); - z(281) = par.ShankLen*z(205); - z(282) = z(199)*z(233) + par.TrunkInertia*z(187)*z(233) + par.ForearmMass*(z(79)*z(270)+z(88)*z(276)) + par.UpparmMass*(z(54)*z(268)+z(60)*z(269)) + par.TrunkMass*(z(26)*z(266)-z(31)*z(267)) - z(281)*z(8); - z(283) = z(200)*z(233) + 2*z(187)*z(239) + par.TrunkMass*z(30)*z(267) + par.ForearmMass*(z(80)*z(270)+z(89)*z(276)) - par.UpparmMass*(z(55)*z(268)-z(61)*z(269)); - z(284) = z(209)*z(269) + par.ForearmMass*(z(81)*z(270)+z(86)*z(276)); - z(285) = z(1)*z(243) + z(2)*z(74); - z(286) = z(1)*z(241) + z(2)*z(49); - z(287) = z(1)*z(49) - z(2)*z(241); - z(288) = z(7)*z(130) + z(7)*z(286) + z(8)*z(287) - z(8)*z(128); - z(289) = z(1)*z(183) + z(2)*z(45); - z(290) = 2*z(137)*z(285) + 2*z(138)*z(245) + 2*z(141)*z(286) + 2*z(142)*z(288) + 2*z(143)*z(183) + 2*z(144)*z(45) - 2*z(140)*z(8) - 2*z(146)*z(243) - 2*z(147)*z(74) - 2*z(148)*z(289) - 2*z(152)*z(10) - 2*z(153)*z(9) - 2*z(155)*z(241) - 2*z(156)*z(49); - z(291) = par.UpparmLen*z(290)/(z(157)^0.5*z(158)^2); - z(292) = z(111)*z(290)/(z(157)^0.5*z(158)^2); - z(293) = z(112)*z(290)/(z(157)^0.5*z(158)^2); - z(294) = par.TrunkLen*z(290)/(z(157)^0.5*z(158)^2); - z(295) = (par.ThighLen*z(10)-z(101)*z(182))/z(102)^0.5; - z(296) = (par.Kseat*z(104)+par.Cseat*(z(257)*q2p+z(295)*q1p))/z(102)^0.5; - z(297) = par.ForearmLen*z(290)/(z(157)^0.5*z(158)^2); - z(298) = par.ShankLen*z(290)/(z(157)^0.5*z(158)^2); - z(299) = par.ThighLen*z(290)/(z(157)^0.5*z(158)^2); - z(300) = z(1)*z(253) + z(2)*z(75); - z(301) = par.a*(par.Kseat*z(104)+par.Cseat*(z(257)*q2p+z(295)*q1p))/z(102)^0.5; - z(302) = z(73)*z(172)*z(276) + z(74)*z(78)*z(168) + z(74)*z(167)*z(270) + z(75)*z(108)*z(168) + z(75)*z(167)*z(276) + z(76)*z(168)*z(270) + z(77)*z(168)*z(276) + z(78)*z(167)*z(243) + z(108)*z(167)*z(253) + z(121)*z(171)*z(270) + z(123)*z(171)*z(276) + 0.5*Fc*z(78)*z(297) + 0.5*Fc*z(72)*z(78)*z(291) + 0.5*Fc*z(78)*z(113)*z(298) + 0.5*Fc*z(78)*z(117)*z(299) + 0.5*Fc*z(108)*z(115)*z(298) + 0.5*Fc*z(108)*z(119)*z(299) + z(9)*z(12)*z(106) + z(9)*z(12)*z(301) + z(10)*z(20)*z(106) + z(10)*z(20)*z(301) + par.ShankLen*z(7)*z(11)*z(106) + par.ShankLen*z(8)*z(10)*z(105) - z(166)*z(270) - z(72)*z(172)*z(270) - z(78)*z(169)*z(285) - z(78)*z(170)*z(245) - z(108)*z(169)*z(300) - z(108)*z(170)*z(263) - z(113)*z(169)*z(270) - z(115)*z(169)*z(276) - z(117)*z(170)*z(270) - z(119)*z(170)*z(276) - 0.5*Fc*z(73)*z(108)*z(291) - 0.5*Fc*z(74)*z(78)*z(292) - 0.5*Fc*z(75)*z(108)*z(292) - 0.5*Fc*z(76)*z(78)*z(293) - 0.5*Fc*z(77)*z(108)*z(293) - 0.5*Fc*z(78)*z(121)*z(294) - 0.5*Fc*z(108)*z(123)*z(294) - z(95)*(z(74)*z(78)+z(75)*z(87)+z(76)*z(270)+z(77)*z(276)) - z(99)*(z(49)*z(53)+z(50)*z(59)+z(51)*z(268)+z(52)*z(269)) - z(98)*(z(25)*z(45)+z(32)*z(46)+z(47)*z(266)-z(48)*z(267)) - z(97)*(z(9)*z(12)+z(10)*z(14)+par.ShankLen*z(7)*z(11)-par.ShankLen*z(8)*z(9)) - z(9)*z(20)*z(296) - z(10)*z(12)*z(105) - z(11)*z(12)*z(296) - z(20)*z(182)*z(105) - par.ShankLen*z(7)*z(9)*z(105) - par.ShankLen*z(8)*z(9)*z(106); - z(303) = z(45)*z(186) + z(46)*z(48) + z(46)*z(184) + z(183)*z(185); - z(304) = z(11)*z(24)*q3p - z(9)*z(23)*q3p - z(23)*z(193) - z(24)*z(190); - z(305) = z(9)*z(24)*q3p - z(10)*z(23)*q3p - z(23)*z(190) - z(24)*z(191); - z(306) = z(45)*q1p*z(304) + z(45)*q2p*z(304) + z(46)*q1p*z(194) + z(46)*q1p*z(235) + z(46)*q2p*z(194) + z(46)*q2p*z(235) + z(48)*q1p*z(195) + z(48)*q2p*z(195) + z(183)*q1p*z(305) + z(183)*q2p*z(305) + z(184)*q1p*z(195) + z(184)*q2p*z(195) + z(185)*q1p*z(197) + z(185)*q2p*z(197) + z(186)*q1p*z(192) + z(186)*q2p*z(192) + 2*z(46)*q3p*z(305) + 2*z(48)*q3p*z(304) + 2*z(185)*q3p*z(195) + 2*z(186)*q3p*z(194); - z(307) = -z(12)*z(23) - z(20)*z(24); - z(308) = z(28)*z(43) + z(44)*z(307); - z(309) = z(43)*z(307) - z(28)*z(44); - z(310) = z(72)*z(308) + z(73)*z(309); - z(311) = -z(23)*z(22) - z(24)*z(16); - z(312) = z(43)*z(34) + z(44)*z(311); - z(313) = z(43)*z(311) - z(44)*z(34); - z(314) = z(72)*z(312) + z(73)*z(313); - z(315) = z(72)*z(309) - z(73)*z(308); - z(316) = z(72)*z(313) - z(73)*z(312); - z(317) = z(303)*z(201) + par.TrunkInertia*z(187)*z(306) + par.ForearmMass*(z(78)*z(314)+z(87)*z(316)+z(310)*z(94)+z(315)*z(91)) + par.TrunkMass*(z(25)*z(34)+z(28)*z(38)+z(32)*z(311)+z(307)*z(34)) + par.UpparmMass*(z(53)*z(312)+z(59)*z(313)+z(308)*z(66)+z(309)*z(63)); - z(318) = z(199)*z(303) + par.TrunkInertia*z(187)*z(303) + 2*par.ForearmMass*(z(78)*z(310)+z(87)*z(315)) + 2*par.TrunkMass*(z(25)*z(28)+z(32)*z(307)) + 2*par.UpparmMass*(z(53)*z(308)+z(59)*z(309)); - z(319) = par.ThighLen*(z(23)*z(43)-z(24)*z(44)); - z(320) = par.ThighLen*(z(23)*z(44)+z(24)*z(43)); - z(321) = z(72)*z(319) - z(73)*z(320); - z(322) = -z(72)*z(320) - z(73)*z(319); - z(323) = z(199)*z(303) + par.TrunkInertia*z(187)*z(303) + par.ForearmMass*(z(78)*z(321)+z(79)*z(310)+z(87)*z(322)+z(88)*z(315)) + par.TrunkMass*(z(26)*z(28)+z(31)*z(307)+par.ThighLen*z(23)*z(25)-par.ThighLen*z(24)*z(32)) - par.UpparmMass*(z(59)*z(320)-z(53)*z(319)-z(54)*z(308)-z(60)*z(309)); - z(324) = 2*z(46)*z(185) + 2*z(48)*z(186); - z(325) = z(200)*z(303) + par.TrunkInertia*z(187)*z(324) + par.ForearmMass*(z(80)*z(310)+z(89)*z(315)) - par.TrunkMass*z(30)*z(307) - par.UpparmMass*(z(55)*z(308)-z(61)*z(309)); - z(326) = z(209)*z(309) + par.ForearmMass*(z(81)*z(310)+z(86)*z(315)); - z(327) = z(43)*z(46) + z(44)*z(185); - z(328) = z(43)*z(185) - z(44)*z(46); - z(329) = z(72)*z(327) + z(73)*z(328); - z(330) = z(43)*z(48) + z(44)*z(186); - z(331) = z(43)*z(186) - z(44)*z(48); + z(136) = z(2)*z(8) - z(1)*z(7); + z(137) = z(10)*z(23) + z(24)*z(136); + z(138) = z(23)*z(136) - z(10)*z(24); + z(139) = -z(9)*z(23) - z(10)*z(24); + z(140) = z(43)*z(46) + z(44)*z(139); + z(141) = z(43)*z(137) + z(44)*z(138); + z(142) = z(43)*z(138) - z(44)*z(137); + z(143) = z(43)*z(139) - z(44)*z(46); + z(144) = -z(43)*z(45) - z(44)*z(46); + z(145) = z(50)*z(72) + z(73)*z(144); + z(146) = z(72)*z(140) + z(73)*z(143); + z(147) = z(72)*z(141) + z(73)*z(142); + z(148) = -z(9)*z(24) - z(11)*z(23); + z(149) = z(43)*z(48) + z(44)*z(148); + z(150) = z(43)*z(148) - z(44)*z(48); + z(151) = -z(43)*z(47) - z(44)*z(48); + z(152) = z(52)*z(72) + z(73)*z(151); + z(153) = z(72)*z(149) + z(73)*z(150); + z(154) = z(1)*(z(76)+z(147)); + z(155) = z(1)*z(145) + z(2)*z(152); + z(156) = z(1)*z(146) + z(2)*z(153); + z(157) = z(1)*z(147) + z(2)*z(74); + z(158) = z(9)*z(76) + z(9)*z(147) + z(10)*z(74) + z(11)*z(74); + z(159) = z(9)*z(145) + z(11)*z(152); + z(160) = z(9)*z(146) + z(11)*z(153); + z(161) = z(1)*(z(51)+z(141)); + z(162) = z(1)*z(140) + z(2)*z(149); + z(163) = z(1)*z(141) + z(2)*z(49); + z(164) = z(2)*(z(51)+z(141)); + z(165) = z(1)*z(49) - z(2)*z(141); + z(166) = z(1)*z(149) - z(2)*z(140); + z(167) = z(7)*z(130) + z(7)*z(163) + z(8)*z(165) - z(8)*z(128); + z(168) = z(7)*z(161) - z(8)*z(164); + z(169) = z(7)*z(162) + z(8)*z(166); + z(170) = z(1)*(z(47)+z(137)); + z(171) = z(1)*z(137) + z(2)*z(45); + z(172) = par.ForearmLen*par.ShankLen; + z(173) = par.ForearmLen*par.ThighLen; + z(174) = par.ShankLen*par.UpparmLen; + z(175) = par.ShankLen*z(111); + z(176) = par.ThighLen*par.UpparmLen; + z(177) = par.TrunkLen*z(111); + z(178) = par.TrunkLen*z(112); + z(179) = par.ForearmLen*z(111); + z(180) = par.ForearmLen*z(112); + z(181) = par.ShankLen*par.TrunkLen; + z(182) = par.ShankLen*z(112); + z(183) = par.ThighLen*z(111); + z(184) = par.ThighLen*z(112); + z(185) = par.UpparmLen*z(111); + z(186) = par.UpparmLen*z(112); + z(187) = par.ForearmLen^2 + par.ShankLen^2 + par.ThighLen^2 + par.TrunkLen^2 + par.UpparmLen^2 + z(111)^2 + z(112)^2; + z(188) = par.ForearmLen*par.UpparmLen; + z(189) = par.ShankLen*par.ThighLen; + z(190) = par.ForearmLen*par.TrunkLen; + z(191) = par.ThighLen*par.TrunkLen; + z(192) = par.TrunkLen*par.UpparmLen; + z(193) = (z(172)*z(154)+z(173)*z(158)+z(174)*z(161)+z(175)*z(2)+z(176)*z(168)+z(177)*z(137)+z(178)*z(45)-z(179)*z(147)-z(180)*z(74)-z(181)*z(170)-z(182)*z(1)-z(183)*z(10)-z(184)*z(9)-z(185)*z(141)-z(186)*z(49))/(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43))^0.5; + z(194) = (z(179)*z(146)+z(180)*z(153)+z(181)*z(125)+z(185)*z(140)+z(186)*z(149)-z(172)*z(156)-z(173)*z(160)-z(174)*z(162)-z(176)*z(169)-z(177)*z(46)-z(178)*z(48)-z(191)*z(24))/(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43))^0.5; + z(195) = (z(179)*z(145)+z(180)*z(152)+z(185)*z(50)+z(186)*z(52)+z(190)*z(123)-z(172)*z(155)-z(173)*z(159)-z(174)*z(129)-z(176)*z(133)-z(192)*z(44))/(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43))^0.5; + z(196) = (z(172)*z(157)+z(173)*z(158)+z(174)*z(163)+z(176)*z(167)+z(177)*z(137)+z(178)*z(45)-z(179)*z(147)-z(180)*z(74)-z(181)*z(171)-z(183)*z(10)-z(184)*z(9)-z(185)*z(141)-z(186)*z(49)-z(189)*z(8))/(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43))^0.5; + z(197) = par.ForearmLen*(par.ShankLen*z(115)+par.ThighLen*z(119)-par.TrunkLen*z(123)-par.UpparmLen*z(73)-z(111)*z(75)-z(112)*z(77))/(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43))^0.5; + z(198) = z(187) + 2*z(172)*z(113) + 2*z(173)*z(117) + 2*z(174)*z(128) + 2*z(176)*z(132) + 2*z(177)*z(45) + 2*z(178)*z(47) + 2*z(188)*z(72) + 2*z(189)*z(7) - 2*z(175)*z(1) - 2*z(179)*z(74) - 2*z(180)*z(76) - 2*z(181)*z(124) - 2*z(182)*z(2) - 2*z(183)*z(9) - 2*z(184)*z(11) - 2*z(185)*z(49) - 2*z(186)*z(51) - 2*z(190)*z(121) - 2*z(191)*z(23) - 2*z(192)*z(43); + z(199) = z(198)^0.5; + z(200) = par.ForearmLen/z(199); + z(201) = z(111)/z(199); + z(202) = z(112)/z(199); + z(203) = par.ShankLen/z(199); + z(204) = par.ThighLen/z(199); + z(205) = par.TrunkLen/z(199); + z(206) = par.UpparmLen/z(199); + z(207) = Fc*z(200); + z(208) = Fc*z(201); + z(209) = Fc*z(202); + z(210) = Fc*z(203); + z(211) = Fc*z(204); + z(212) = Fc*z(205); + z(213) = Fc*z(206); + z(214) = par.ShankCM*z(96); + z(215) = z(73)*z(108)*z(213) + z(74)*z(78)*z(208) + z(75)*z(108)*z(208) + z(76)*z(78)*z(209) + z(77)*z(108)*z(209) + z(78)*z(121)*z(212) + z(108)*z(123)*z(212) + z(9)*z(20)*z(106) + z(11)*z(12)*z(106) - z(214)*z(1) - z(78)*z(207) - z(72)*z(78)*z(213) - z(78)*z(113)*z(210) - z(78)*z(117)*z(211) - z(108)*z(115)*z(210) - z(108)*z(119)*z(211) - z(95)*(z(76)*z(78)+z(77)*z(87)) - z(97)*(z(9)*z(14)+z(11)*z(12)) - z(98)*(z(25)*z(47)+z(32)*z(48)) - z(99)*(z(51)*z(53)+z(52)*z(59)) - z(9)*z(12)*z(105) - z(10)*z(20)*z(105); + z(216) = par.ThighCM*z(97); + z(217) = z(73)*z(109)*z(213) + z(74)*z(79)*z(208) + z(75)*z(109)*z(208) + z(76)*z(79)*z(209) + z(77)*z(109)*z(209) + z(79)*z(121)*z(212) + z(109)*z(123)*z(212) + par.ThighLen*(z(9)*z(106)-z(10)*z(105)) - z(216)*z(9) - z(79)*z(207) - z(72)*z(79)*z(213) - z(79)*z(113)*z(210) - z(79)*z(117)*z(211) - z(109)*z(115)*z(210) - z(109)*z(119)*z(211) - z(95)*(z(76)*z(79)+z(77)*z(88)) - z(98)*(z(26)*z(47)+z(31)*z(48)) - z(99)*(z(51)*z(54)+z(52)*z(60)); + z(218) = z(98)*z(30)*z(48) + z(73)*z(110)*z(213) + z(74)*z(80)*z(208) + z(75)*z(110)*z(208) + z(76)*z(80)*z(209) + z(77)*z(110)*z(209) + z(80)*z(121)*z(212) + z(110)*z(123)*z(212) + z(99)*(z(51)*z(55)-z(52)*z(61)) - z(80)*z(207) - z(72)*z(80)*z(213) - z(80)*z(113)*z(210) - z(80)*z(117)*z(211) - z(110)*z(115)*z(210) - z(110)*z(119)*z(211) - z(95)*(z(76)*z(80)+z(77)*z(89)); + z(219) = par.UpparmCM*z(99); + z(220) = z(73)*z(107)*z(213) + z(74)*z(81)*z(208) + z(75)*z(107)*z(208) + z(76)*z(81)*z(209) + z(77)*z(107)*z(209) + z(81)*z(121)*z(212) + z(107)*z(123)*z(212) - z(219)*z(52) - z(81)*z(207) - z(72)*z(81)*z(213) - z(81)*z(113)*z(210) - z(81)*z(117)*z(211) - z(107)*z(115)*z(210) - z(107)*z(119)*z(211) - z(95)*(z(76)*z(81)+z(77)*z(86)); + z(221) = par.ForearmCM*z(95); + z(222) = -z(221)*z(77) - par.ForearmLen*(z(115)*z(210)+z(119)*z(211)-z(73)*z(213)-z(75)*z(208)-z(77)*z(209)-z(123)*z(212)); + z(223) = z(45)*z(48) + z(46)*z(137); + z(224) = z(46)^2 + z(48)^2; + z(226) = -z(1)*z(8)*q1p - z(1)*z(8)*q2p - z(2)*z(7)*q1p - z(2)*z(7)*q2p; + z(227) = z(2)*z(8)*q1p + z(2)*z(8)*q2p - z(1)*z(7)*q1p - z(1)*z(7)*q2p; + z(228) = z(10)*z(23)*q3p + z(23)*z(226) + z(24)*z(227) - z(9)*z(24)*q3p; + z(229) = z(1)*z(7)*q1p + z(1)*z(7)*q2p - z(2)*z(8)*q1p - z(2)*z(8)*q2p; + z(230) = z(23)*z(226) - z(9)*z(24)*q3p - z(11)*z(23)*q3p - z(24)*z(229); + z(231) = z(23)*z(227) - z(9)*z(23)*q3p - z(10)*z(24)*q3p - z(24)*z(226); + z(232) = z(1)*z(8)*q1p + z(1)*z(8)*q2p + z(2)*z(7)*q1p + z(2)*z(7)*q2p; + z(233) = z(23)*z(136)*q3p + z(23)*z(227) + z(24)*z(232) - z(10)*z(24)*q3p; + z(234) = z(45)*q1p*z(230) + z(45)*q2p*z(230) + z(46)*q1p*z(233) + z(46)*q2p*z(233) + z(48)*q1p*z(228) + z(48)*q2p*z(228) + z(137)*q1p*z(231) + z(137)*q2p*z(231) + 2*z(46)*q3p*z(231) + 2*z(48)*q3p*z(230); + z(235) = par.TrunkInertia*z(223); + z(236) = par.TrunkInertia*z(224); + z(237) = par.TrunkInertia*z(234); + z(238) = par.ForearmInertia + par.ShankInertia + par.ThighInertia + par.UpparmInertia + par.ShankMass*par.ShankCM^2; + z(239) = z(238) + z(223)*z(235) + par.ForearmMass*(z(78)^2+z(87)^2) + par.ThighMass*(z(12)^2+z(14)^2) + par.TrunkMass*(z(25)^2+z(32)^2) + par.UpparmMass*(z(53)^2+z(59)^2); + z(240) = par.ForearmInertia + par.ThighInertia + par.UpparmInertia; + z(241) = par.ThighCM*par.ThighMass; + z(242) = z(240) + z(241)*z(14) + z(223)*z(235) + par.ForearmMass*(z(78)*z(79)+z(87)*z(88)) + par.TrunkMass*(z(25)*z(26)+z(31)*z(32)) + par.UpparmMass*(z(53)*z(54)+z(59)*z(60)); + z(243) = z(240) + z(223)*z(236) + par.ForearmMass*(z(78)*z(80)+z(87)*z(89)) - par.TrunkMass*z(30)*z(32) - par.UpparmMass*(z(53)*z(55)-z(59)*z(61)); + z(244) = par.ForearmInertia + par.UpparmInertia; + z(245) = par.UpparmCM*par.UpparmMass; + z(246) = z(244) + z(245)*z(59) + par.ForearmMass*(z(78)*z(81)+z(86)*z(87)); + z(247) = par.ForearmCM*par.ForearmMass; + z(248) = par.ForearmInertia + z(247)*z(87); + z(249) = z(223)*z(237) + par.ForearmMass*(z(78)*z(94)+z(87)*z(91)) + par.ThighMass*(z(12)*z(19)+z(14)*z(16)) + par.TrunkMass*(z(25)*z(38)+z(32)*z(34)) + par.UpparmMass*(z(53)*z(66)+z(59)*z(63)); + z(250) = par.ForearmInertia + par.ThighInertia + par.UpparmInertia + par.ThighMass*par.ThighCM^2; + z(251) = z(250) + z(223)*z(235) + par.ForearmMass*(z(79)^2+z(88)^2) + par.TrunkMass*(z(26)^2+z(31)^2) + par.UpparmMass*(z(54)^2+z(60)^2); + z(252) = z(240) + z(223)*z(236) + par.ForearmMass*(z(79)*z(80)+z(88)*z(89)) - par.TrunkMass*z(30)*z(31) - par.UpparmMass*(z(54)*z(55)-z(60)*z(61)); + z(253) = z(244) + z(245)*z(60) + par.ForearmMass*(z(79)*z(81)+z(86)*z(88)); + z(254) = par.ForearmInertia + z(247)*z(88); + z(255) = z(241)*z(16) + z(223)*z(237) + par.ForearmMass*(z(79)*z(94)+z(88)*z(91)) + par.TrunkMass*(z(26)*z(38)+z(31)*z(34)) + par.UpparmMass*(z(54)*z(66)+z(60)*z(63)); + z(256) = z(240) + z(224)*z(235) + par.ForearmMass*(z(78)*z(80)+z(87)*z(89)) - par.TrunkMass*z(30)*z(32) - par.UpparmMass*(z(53)*z(55)-z(59)*z(61)); + z(257) = z(240) + z(224)*z(235) + par.ForearmMass*(z(79)*z(80)+z(88)*z(89)) - par.TrunkMass*z(30)*z(31) - par.UpparmMass*(z(54)*z(55)-z(60)*z(61)); + z(258) = z(240) + z(224)*z(236) + par.TrunkMass*z(30)^2 + par.ForearmMass*(z(80)^2+z(89)^2) + par.UpparmMass*(z(55)^2+z(61)^2); + z(259) = z(244) + z(245)*z(61) + par.ForearmMass*(z(80)*z(81)+z(86)*z(89)); + z(260) = par.ForearmInertia + z(247)*z(89); + z(261) = z(224)*z(237) + par.ForearmMass*(z(80)*z(94)+z(89)*z(91)) - par.TrunkMass*z(30)*z(34) - par.UpparmMass*(z(55)*z(66)-z(61)*z(63)); + z(262) = par.ForearmInertia + par.UpparmInertia + par.UpparmMass*par.UpparmCM^2; + z(263) = z(262) + par.ForearmMass*(z(81)^2+z(86)^2); + z(264) = par.ForearmInertia + z(247)*z(86); + z(265) = z(245)*z(63) + par.ForearmMass*(z(81)*z(94)+z(86)*z(91)); + z(266) = par.ForearmInertia + par.ForearmMass*par.ForearmCM^2; + z(267) = z(247)*z(91); + z(268) = z(11)*z(24) + z(23)*z(136); + z(269) = z(45)*z(46) + z(46)*z(268) + z(48)*z(137) + z(137)*z(138); + z(270) = z(11)*z(23)*q3p + z(23)*z(232) + z(24)*z(229) - z(24)*z(136)*q3p; + z(271) = z(23)*z(232) - z(10)*z(23)*q3p - z(24)*z(136)*q3p - z(24)*z(227); + z(272) = z(45)*q1p*z(231) + z(45)*q2p*z(231) + z(46)*q1p*z(228) + z(46)*q1p*z(270) + z(46)*q2p*z(228) + z(46)*q2p*z(270) + z(48)*q1p*z(233) + z(48)*q2p*z(233) + z(137)*q1p*z(230) + z(137)*q1p*z(271) + z(137)*q2p*z(230) + z(137)*q2p*z(271) + z(138)*q1p*z(233) + z(138)*q2p*z(233) + z(268)*q1p*z(231) + z(268)*q2p*z(231) + 2*z(46)*q3p*z(230) + 2*z(46)*q3p*z(271) + 2*z(48)*q3p*z(231) + 2*z(138)*q3p*z(231); + z(273) = z(269)*z(237) + par.TrunkInertia*z(223)*z(272); + z(274) = z(269)*(z(235)+par.TrunkInertia*z(223)); + z(275) = par.TrunkInertia*z(46)*(z(48)+z(138)); + z(276) = z(236)*z(269) + 2*z(223)*z(275); + z(277) = 2*z(172)*z(154) + 2*z(173)*z(158) + 2*z(174)*z(161) + 2*z(175)*z(2) + 2*z(176)*z(168) + 2*z(177)*z(137) + 2*z(178)*z(45) - 2*z(179)*z(147) - 2*z(180)*z(74) - 2*z(181)*z(170) - 2*z(182)*z(1) - 2*z(183)*z(10) - 2*z(184)*z(9) - 2*z(185)*z(141) - 2*z(186)*z(49); + z(278) = par.UpparmLen*z(277)/(z(198)^0.5*z(199)^2); + z(279) = z(111)*z(277)/(z(198)^0.5*z(199)^2); + z(280) = z(72)*z(142) - z(73)*z(141); + z(281) = z(112)*z(277)/(z(198)^0.5*z(199)^2); + z(282) = par.TrunkLen*z(277)/(z(198)^0.5*z(199)^2); + z(283) = (par.ThighLen*z(10)+z(100)*z(1)-par.ShankLen*z(2)-z(101)*z(136))/z(102)^0.5; + z(284) = par.ThighLen*(z(10)-par.a*z(136))/z(102)^0.5; + z(285) = (par.Kseat*z(103)+par.Cseat*(z(283)*q1p+z(284)*q2p))/z(102)^0.5; + z(286) = par.ForearmLen*z(277)/(z(198)^0.5*z(199)^2); + z(287) = par.ShankLen*z(277)/(z(198)^0.5*z(199)^2); + z(288) = par.ThighLen*z(277)/(z(198)^0.5*z(199)^2); + z(289) = z(1)*(z(77)+z(280)); + z(290) = z(9)*z(77) + z(9)*z(280) + z(10)*z(75) + z(11)*z(75); + z(291) = par.a*(par.Kseat*z(103)+par.Cseat*(z(283)*q1p+z(284)*q2p))/z(102)^0.5; + z(292) = z(214)*z(2) + z(74)*z(78)*z(209) + z(75)*z(108)*z(209) + z(78)*z(147)*z(208) + z(108)*z(208)*z(280) + 0.5*Fc*z(78)*z(286) + 0.5*Fc*z(72)*z(78)*z(278) + 0.5*Fc*z(78)*z(113)*z(287) + 0.5*Fc*z(78)*z(117)*z(288) + 0.5*Fc*z(108)*z(115)*z(287) + 0.5*Fc*z(108)*z(119)*z(288) + z(9)*z(12)*z(106) + z(9)*z(12)*z(291) + z(10)*z(20)*z(106) + z(10)*z(20)*z(291) - z(78)*z(154)*z(210) - z(78)*z(158)*z(211) - z(108)*z(210)*z(289) - z(108)*z(211)*z(290) - 0.5*Fc*z(73)*z(108)*z(278) - 0.5*Fc*z(74)*z(78)*z(279) - 0.5*Fc*z(75)*z(108)*z(279) - 0.5*Fc*z(76)*z(78)*z(281) - 0.5*Fc*z(77)*z(108)*z(281) - 0.5*Fc*z(78)*z(121)*z(282) - 0.5*Fc*z(108)*z(123)*z(282) - z(95)*(z(74)*z(78)+z(75)*z(87)) - z(97)*(z(9)*z(12)+z(10)*z(14)) - z(98)*(z(25)*z(45)+z(32)*z(46)) - z(99)*(z(49)*z(53)+z(50)*z(59)) - z(9)*z(20)*z(285) - z(10)*z(12)*z(105) - z(11)*z(12)*z(285) - z(20)*z(136)*z(105); + z(293) = par.ShankLen*(z(7)*z(23)-z(8)*z(24)); + z(294) = par.ShankLen*(z(7)*z(24)+z(8)*z(23)); + z(295) = z(43)*z(293) - z(44)*z(294); + z(296) = -z(43)*z(294) - z(44)*z(293); + z(297) = z(72)*z(295) + z(73)*z(296); + z(298) = (z(7)*z(24)+z(8)*z(23))*z(6); + z(299) = (z(7)*z(23)-z(8)*z(24))*z(6); + z(300) = z(43)*z(298) + z(44)*z(299); + z(301) = z(43)*z(299) - z(44)*z(298); + z(302) = z(72)*z(300) + z(73)*z(301); + z(303) = z(72)*z(296) - z(73)*z(295); + z(304) = z(72)*z(301) - z(73)*z(300); + z(305) = z(269)*z(237) + par.TrunkInertia*z(223)*z(272) + par.ForearmMass*(z(78)*z(302)+z(87)*z(304)+z(297)*z(94)+z(303)*z(91)) + par.UpparmMass*(z(53)*z(300)+z(59)*z(301)+z(295)*z(66)+z(296)*z(63)) + par.TrunkMass*(z(25)*z(298)+z(32)*z(299)+z(293)*z(38)-z(294)*z(34)) - par.ThighMass*(par.ShankLen*z(8)*z(16)-par.ShankLen*z(7)*z(19)-z(7)*z(14)*z(6)-z(8)*z(12)*z(6)); + z(306) = par.ShankLen*par.ThighMass; + z(307) = z(235)*z(269) + par.TrunkInertia*z(223)*z(269) + 2*par.ForearmMass*(z(78)*z(297)+z(87)*z(303)) + 2*par.UpparmMass*(z(53)*z(295)+z(59)*z(296)) + 2*par.TrunkMass*(z(25)*z(293)-z(32)*z(294)) + 2*z(306)*(z(7)*z(12)-z(8)*z(14)); + z(308) = par.ShankLen*z(241); + z(309) = z(235)*z(269) + par.TrunkInertia*z(223)*z(269) + par.ForearmMass*(z(79)*z(297)+z(88)*z(303)) + par.UpparmMass*(z(54)*z(295)+z(60)*z(296)) + par.TrunkMass*(z(26)*z(293)-z(31)*z(294)) - z(308)*z(8); + z(310) = z(236)*z(269) + 2*z(223)*z(275) + par.TrunkMass*z(30)*z(294) + par.ForearmMass*(z(80)*z(297)+z(89)*z(303)) - par.UpparmMass*(z(55)*z(295)-z(61)*z(296)); + z(311) = z(245)*z(296) + par.ForearmMass*(z(81)*z(297)+z(86)*z(303)); + z(312) = 2*z(172)*z(157) + 2*z(173)*z(158) + 2*z(174)*z(163) + 2*z(176)*z(167) + 2*z(177)*z(137) + 2*z(178)*z(45) - 2*z(179)*z(147) - 2*z(180)*z(74) - 2*z(181)*z(171) - 2*z(183)*z(10) - 2*z(184)*z(9) - 2*z(185)*z(141) - 2*z(186)*z(49) - 2*z(189)*z(8); + z(313) = par.UpparmLen*z(312)/(z(198)^0.5*z(199)^2); + z(314) = z(111)*z(312)/(z(198)^0.5*z(199)^2); + z(315) = z(112)*z(312)/(z(198)^0.5*z(199)^2); + z(316) = par.TrunkLen*z(312)/(z(198)^0.5*z(199)^2); + z(317) = (par.ThighLen*z(10)-z(101)*z(136))/z(102)^0.5; + z(318) = (par.Kseat*z(104)+par.Cseat*(z(284)*q2p+z(317)*q1p))/z(102)^0.5; + z(319) = par.ForearmLen*z(312)/(z(198)^0.5*z(199)^2); + z(320) = par.ShankLen*z(312)/(z(198)^0.5*z(199)^2); + z(321) = par.ThighLen*z(312)/(z(198)^0.5*z(199)^2); + z(322) = z(1)*z(280) + z(2)*z(75); + z(323) = par.a*(par.Kseat*z(104)+par.Cseat*(z(284)*q2p+z(317)*q1p))/z(102)^0.5; + z(324) = z(73)*z(213)*z(303) + z(74)*z(78)*z(209) + z(74)*z(208)*z(297) + z(75)*z(108)*z(209) + z(75)*z(208)*z(303) + z(76)*z(209)*z(297) + z(77)*z(209)*z(303) + z(78)*z(147)*z(208) + z(108)*z(208)*z(280) + z(121)*z(212)*z(297) + z(123)*z(212)*z(303) + 0.5*Fc*z(78)*z(319) + 0.5*Fc*z(72)*z(78)*z(313) + 0.5*Fc*z(78)*z(113)*z(320) + 0.5*Fc*z(78)*z(117)*z(321) + 0.5*Fc*z(108)*z(115)*z(320) + 0.5*Fc*z(108)*z(119)*z(321) + z(9)*z(12)*z(106) + z(9)*z(12)*z(323) + z(10)*z(20)*z(106) + z(10)*z(20)*z(323) + par.ShankLen*z(7)*z(11)*z(106) + par.ShankLen*z(8)*z(10)*z(105) - z(207)*z(297) - z(72)*z(213)*z(297) - z(78)*z(157)*z(210) - z(78)*z(158)*z(211) - z(108)*z(210)*z(322) - z(108)*z(211)*z(290) - z(113)*z(210)*z(297) - z(115)*z(210)*z(303) - z(117)*z(211)*z(297) - z(119)*z(211)*z(303) - 0.5*Fc*z(73)*z(108)*z(313) - 0.5*Fc*z(74)*z(78)*z(314) - 0.5*Fc*z(75)*z(108)*z(314) - 0.5*Fc*z(76)*z(78)*z(315) - 0.5*Fc*z(77)*z(108)*z(315) - 0.5*Fc*z(78)*z(121)*z(316) - 0.5*Fc*z(108)*z(123)*z(316) - z(95)*(z(74)*z(78)+z(75)*z(87)+z(76)*z(297)+z(77)*z(303)) - z(99)*(z(49)*z(53)+z(50)*z(59)+z(51)*z(295)+z(52)*z(296)) - z(98)*(z(25)*z(45)+z(32)*z(46)+z(47)*z(293)-z(48)*z(294)) - z(97)*(z(9)*z(12)+z(10)*z(14)+par.ShankLen*z(7)*z(11)-par.ShankLen*z(8)*z(9)) - z(9)*z(20)*z(318) - z(10)*z(12)*z(105) - z(11)*z(12)*z(318) - z(20)*z(136)*z(105) - par.ShankLen*z(7)*z(9)*z(105) - par.ShankLen*z(8)*z(9)*z(106); + z(325) = z(45)*z(148) + z(46)*z(48) + z(46)*z(138) + z(137)*z(139); + z(326) = z(11)*z(24)*q3p - z(9)*z(23)*q3p - z(23)*z(229) - z(24)*z(226); + z(327) = z(9)*z(24)*q3p - z(10)*z(23)*q3p - z(23)*z(226) - z(24)*z(227); + z(328) = z(45)*q1p*z(326) + z(45)*q2p*z(326) + z(46)*q1p*z(230) + z(46)*q1p*z(271) + z(46)*q2p*z(230) + z(46)*q2p*z(271) + z(48)*q1p*z(231) + z(48)*q2p*z(231) + z(137)*q1p*z(327) + z(137)*q2p*z(327) + z(138)*q1p*z(231) + z(138)*q2p*z(231) + z(139)*q1p*z(233) + z(139)*q2p*z(233) + z(148)*q1p*z(228) + z(148)*q2p*z(228) + 2*z(46)*q3p*z(327) + 2*z(48)*q3p*z(326) + 2*z(139)*q3p*z(231) + 2*z(148)*q3p*z(230); + z(329) = -z(12)*z(23) - z(20)*z(24); + z(330) = z(28)*z(43) + z(44)*z(329); + z(331) = z(43)*z(329) - z(28)*z(44); z(332) = z(72)*z(330) + z(73)*z(331); - z(333) = z(1)*z(329) + z(2)*z(332); - z(334) = z(9)*z(329) + z(11)*z(332); - z(335) = z(1)*z(327) + z(2)*z(330); - z(336) = z(1)*z(330) - z(2)*z(327); - z(337) = z(7)*z(335) + z(8)*z(336); - z(338) = 2*z(137)*z(333) + 2*z(138)*z(334) + 2*z(141)*z(335) + 2*z(142)*z(337) + 2*z(143)*z(46) + 2*z(144)*z(48) + 2*z(151)*z(24) - 2*z(146)*z(329) - 2*z(147)*z(332) - 2*z(148)*z(125) - 2*z(155)*z(327) - 2*z(156)*z(330); - z(339) = par.UpparmLen*z(338)/(z(157)^0.5*z(158)^2); - z(340) = z(111)*z(338)/(z(157)^0.5*z(158)^2); - z(341) = z(72)*z(328) - z(73)*z(327); - z(342) = z(112)*z(338)/(z(157)^0.5*z(158)^2); - z(343) = z(72)*z(331) - z(73)*z(330); - z(344) = par.TrunkLen*z(338)/(z(157)^0.5*z(158)^2); - z(345) = par.ForearmLen*z(338)/(z(157)^0.5*z(158)^2); - z(346) = par.ShankLen*z(338)/(z(157)^0.5*z(158)^2); - z(347) = par.ThighLen*z(338)/(z(157)^0.5*z(158)^2); - z(348) = z(1)*z(341) + z(2)*z(343); - z(349) = z(9)*z(341) + z(11)*z(343); - z(350) = z(73)*z(172)*z(315) + z(74)*z(167)*z(310) + z(75)*z(167)*z(315) + z(76)*z(168)*z(310) + z(77)*z(168)*z(315) + z(78)*z(167)*z(329) + z(78)*z(168)*z(332) + z(108)*z(167)*z(341) + z(108)*z(168)*z(343) + z(121)*z(171)*z(310) + z(123)*z(171)*z(315) + 0.5*Fc*z(78)*z(345) + 0.5*Fc*z(72)*z(78)*z(339) + 0.5*Fc*z(78)*z(113)*z(346) + 0.5*Fc*z(78)*z(117)*z(347) + 0.5*Fc*z(108)*z(115)*z(346) + 0.5*Fc*z(108)*z(119)*z(347) - z(166)*z(310) - z(72)*z(172)*z(310) - z(78)*z(169)*z(333) - z(78)*z(170)*z(334) - z(108)*z(169)*z(348) - z(108)*z(170)*z(349) - z(113)*z(169)*z(310) - z(115)*z(169)*z(315) - z(117)*z(170)*z(310) - z(119)*z(170)*z(315) - 0.5*Fc*z(73)*z(108)*z(339) - 0.5*Fc*z(74)*z(78)*z(340) - 0.5*Fc*z(75)*z(108)*z(340) - 0.5*Fc*z(76)*z(78)*z(342) - 0.5*Fc*z(77)*z(108)*z(342) - 0.5*Fc*z(78)*z(121)*z(344) - 0.5*Fc*z(108)*z(123)*z(344) - z(95)*(z(76)*z(310)+z(77)*z(315)+z(78)*z(332)+z(87)*z(343)) - z(98)*(z(25)*z(48)+z(28)*z(47)+z(32)*z(186)+z(48)*z(307)) - z(99)*(z(51)*z(308)+z(52)*z(309)+z(53)*z(330)+z(59)*z(331)); - z(351) = -z(25)*z(43) - z(41)*z(44); - z(352) = z(56)*z(72) + z(73)*z(351); - z(353) = -z(43)*z(42) - z(44)*z(34); - z(354) = z(72)*z(63) + z(73)*z(353); - z(355) = z(72)*z(351) - z(56)*z(73); - z(356) = z(72)*z(353) - z(73)*z(63); - z(357) = par.ForearmMass*(z(78)*z(354)+z(87)*z(356)+z(352)*z(94)+z(355)*z(91)) + par.UpparmMass*(z(53)*z(63)+z(56)*z(66)+z(59)*z(353)+z(351)*z(63)); - z(358) = 2*par.ForearmMass*(z(78)*z(352)+z(87)*z(355)) + 2*par.UpparmMass*(z(53)*z(56)+z(59)*z(351)); - z(359) = -z(26)*z(43) - z(40)*z(44); - z(360) = z(57)*z(72) + z(73)*z(359); - z(361) = z(72)*z(359) - z(57)*z(73); - z(362) = par.ForearmMass*(z(78)*z(360)+z(79)*z(352)+z(87)*z(361)+z(88)*z(355)) + par.UpparmMass*(z(53)*z(57)+z(54)*z(56)+z(59)*z(359)+z(60)*z(351)); - z(363) = z(39)*(z(43)*z(72)-z(44)*z(73)); - z(364) = z(39)*(z(43)*z(73)+z(44)*z(72)); - z(365) = -par.ForearmMass*(z(78)*z(363)-z(80)*z(352)-z(87)*z(364)-z(89)*z(355)) - par.UpparmMass*(z(55)*z(56)+z(39)*z(43)*z(53)-z(61)*z(351)-z(39)*z(44)*z(59)); - z(366) = z(209)*z(351) + par.ForearmMass*(z(81)*z(352)+z(86)*z(355)); - z(367) = -z(43)*z(45) - z(44)*z(46); - z(368) = z(50)*z(72) + z(73)*z(367); - z(369) = -z(43)*z(47) - z(44)*z(48); - z(370) = z(52)*z(72) + z(73)*z(369); - z(371) = z(1)*z(368) + z(2)*z(370); - z(372) = z(9)*z(368) + z(11)*z(370); - z(373) = 2*z(137)*z(371) + 2*z(138)*z(372) + 2*z(141)*z(129) + 2*z(142)*z(133) + 2*z(154)*z(44) - 2*z(145)*z(123) - 2*z(146)*z(368) - 2*z(147)*z(370) - 2*z(155)*z(50) - 2*z(156)*z(52); - z(374) = par.UpparmLen*z(373)/(z(157)^0.5*z(158)^2); - z(375) = z(111)*z(373)/(z(157)^0.5*z(158)^2); - z(376) = z(72)*z(367) - z(50)*z(73); - z(377) = z(112)*z(373)/(z(157)^0.5*z(158)^2); - z(378) = z(72)*z(369) - z(52)*z(73); - z(379) = par.TrunkLen*z(373)/(z(157)^0.5*z(158)^2); - z(380) = z(44)*z(73) - z(43)*z(72); - z(381) = par.ForearmLen*z(373)/(z(157)^0.5*z(158)^2); - z(382) = par.ShankLen*z(373)/(z(157)^0.5*z(158)^2); - z(383) = par.ThighLen*z(373)/(z(157)^0.5*z(158)^2); - z(384) = z(1)*z(376) + z(2)*z(378); - z(385) = z(9)*z(376) + z(11)*z(378); - z(386) = z(73)*z(172)*z(355) + z(74)*z(167)*z(352) + z(75)*z(167)*z(355) + z(76)*z(168)*z(352) + z(77)*z(168)*z(355) + z(78)*z(123)*z(171) + z(78)*z(167)*z(368) + z(78)*z(168)*z(370) + z(108)*z(167)*z(376) + z(108)*z(168)*z(378) + z(108)*z(171)*z(380) + z(121)*z(171)*z(352) + z(123)*z(171)*z(355) + 0.5*Fc*z(78)*z(381) + 0.5*Fc*z(72)*z(78)*z(374) + 0.5*Fc*z(78)*z(113)*z(382) + 0.5*Fc*z(78)*z(117)*z(383) + 0.5*Fc*z(108)*z(115)*z(382) + 0.5*Fc*z(108)*z(119)*z(383) - z(166)*z(352) - z(72)*z(172)*z(352) - z(78)*z(169)*z(371) - z(78)*z(170)*z(372) - z(108)*z(169)*z(384) - z(108)*z(170)*z(385) - z(113)*z(169)*z(352) - z(115)*z(169)*z(355) - z(117)*z(170)*z(352) - z(119)*z(170)*z(355) - 0.5*Fc*z(73)*z(108)*z(374) - 0.5*Fc*z(74)*z(78)*z(375) - 0.5*Fc*z(75)*z(108)*z(375) - 0.5*Fc*z(76)*z(78)*z(377) - 0.5*Fc*z(77)*z(108)*z(377) - 0.5*Fc*z(78)*z(121)*z(379) - 0.5*Fc*z(108)*z(123)*z(379) - z(95)*(z(76)*z(352)+z(77)*z(355)+z(78)*z(370)+z(87)*z(378)) - z(99)*(z(51)*z(56)+z(52)*z(53)+z(52)*z(351)+z(59)*z(369)); - z(387) = -z(53)*z(72) - z(67)*z(73); - z(388) = -z(72)*z(71) - z(73)*z(63); - z(389) = par.ForearmMass*(z(78)*z(91)+z(83)*z(94)+z(87)*z(388)+z(387)*z(91)); - z(390) = par.ForearmMass*(z(78)*z(83)+z(87)*z(387)); - z(391) = -z(54)*z(72) - z(68)*z(73); - z(392) = par.ForearmMass*(z(78)*z(84)+z(79)*z(83)+z(87)*z(391)+z(88)*z(387)); - z(393) = z(55)*z(72) - z(69)*z(73); - z(394) = par.ForearmMass*(z(78)*z(85)+z(80)*z(83)+z(87)*z(393)+z(89)*z(387)); - z(395) = par.ForearmMass*(z(81)*z(83)+z(86)*z(387)+par.UpparmLen*z(72)*z(78)-par.UpparmLen*z(73)*z(87)); - z(396) = 2*z(137)*z(115) + 2*z(138)*z(119) - 2*z(139)*z(73) - 2*z(145)*z(123) - 2*z(146)*z(75) - 2*z(147)*z(77); - z(397) = par.UpparmLen*z(396)/(z(157)^0.5*z(158)^2); - z(398) = z(111)*z(396)/(z(157)^0.5*z(158)^2); - z(399) = -z(49)*z(72) - z(50)*z(73); - z(400) = z(112)*z(396)/(z(157)^0.5*z(158)^2); - z(401) = -z(51)*z(72) - z(52)*z(73); - z(402) = par.TrunkLen*z(396)/(z(157)^0.5*z(158)^2); - z(403) = par.ForearmLen*z(396)/(z(157)^0.5*z(158)^2); - z(404) = par.ShankLen*z(396)/(z(157)^0.5*z(158)^2); - z(405) = par.ThighLen*z(396)/(z(157)^0.5*z(158)^2); - z(406) = z(1)*z(399) + z(2)*z(401); - z(407) = z(9)*z(399) + z(11)*z(401); - z(408) = z(72)*z(108)*z(172) + z(73)*z(78)*z(172) + z(73)*z(172)*z(387) + z(74)*z(83)*z(167) + z(75)*z(78)*z(167) + z(75)*z(167)*z(387) + z(76)*z(83)*z(168) + z(77)*z(78)*z(168) + z(77)*z(168)*z(387) + z(78)*z(123)*z(171) + z(83)*z(121)*z(171) + z(108)*z(167)*z(399) + z(108)*z(168)*z(401) + z(108)*z(171)*z(380) + z(123)*z(171)*z(387) + 0.5*Fc*z(78)*z(403) + 0.5*Fc*z(72)*z(78)*z(397) + 0.5*Fc*z(78)*z(113)*z(404) + 0.5*Fc*z(78)*z(117)*z(405) + 0.5*Fc*z(108)*z(115)*z(404) + 0.5*Fc*z(108)*z(119)*z(405) - z(83)*z(166) - z(72)*z(83)*z(172) - z(78)*z(115)*z(169) - z(78)*z(119)*z(170) - z(83)*z(113)*z(169) - z(83)*z(117)*z(170) - z(108)*z(169)*z(406) - z(108)*z(170)*z(407) - z(115)*z(169)*z(387) - z(119)*z(170)*z(387) - 0.5*Fc*z(73)*z(108)*z(397) - 0.5*Fc*z(74)*z(78)*z(398) - 0.5*Fc*z(75)*z(108)*z(398) - 0.5*Fc*z(76)*z(78)*z(400) - 0.5*Fc*z(77)*z(108)*z(400) - 0.5*Fc*z(78)*z(121)*z(402) - 0.5*Fc*z(108)*z(123)*z(402) - z(95)*(z(76)*z(83)+z(77)*z(78)+z(77)*z(387)+z(87)*z(401)); - z(409) = z(74)*z(79)*z(168) + z(75)*z(109)*z(168) + z(79)*z(167)*z(243) + z(109)*z(167)*z(253) + 0.5*Fc*z(79)*z(259) + 0.5*Fc*z(72)*z(79)*z(251) + 0.5*Fc*z(79)*z(113)*z(260) + 0.5*Fc*z(79)*z(117)*z(261) + 0.5*Fc*z(109)*z(115)*z(260) + 0.5*Fc*z(109)*z(119)*z(261) + par.ThighLen*(z(10)*z(106)+z(10)*z(264)-z(9)*z(258)-z(182)*z(105)) - z(175)*z(10) - z(79)*z(169)*z(244) - z(79)*z(170)*z(245) - z(109)*z(169)*z(262) - z(109)*z(170)*z(263) - 0.5*Fc*z(73)*z(109)*z(251) - 0.5*Fc*z(74)*z(79)*z(252) - 0.5*Fc*z(75)*z(109)*z(252) - 0.5*Fc*z(76)*z(79)*z(254) - 0.5*Fc*z(77)*z(109)*z(254) - 0.5*Fc*z(79)*z(121)*z(255) - 0.5*Fc*z(109)*z(123)*z(255) - z(95)*(z(74)*z(79)+z(75)*z(88)) - z(98)*(z(26)*z(45)+z(31)*z(46)) - z(99)*(z(49)*z(54)+z(50)*z(60)); - z(410) = z(233)*z(201) + par.TrunkInertia*z(187)*z(236) + z(205)*z(7)*z(6) + par.ForearmMass*(z(79)*z(275)+z(88)*z(277)) + par.TrunkMass*(z(26)*z(271)+z(31)*z(272)) + par.UpparmMass*(z(54)*z(273)+z(60)*z(274)); - z(411) = z(74)*z(79)*z(168) + z(75)*z(109)*z(168) + z(79)*z(167)*z(243) + z(109)*z(167)*z(253) + 0.5*Fc*z(79)*z(297) + 0.5*Fc*z(72)*z(79)*z(291) + 0.5*Fc*z(79)*z(113)*z(298) + 0.5*Fc*z(79)*z(117)*z(299) + 0.5*Fc*z(109)*z(115)*z(298) + 0.5*Fc*z(109)*z(119)*z(299) + par.ThighLen*(z(10)*z(106)+z(10)*z(301)-z(9)*z(296)-z(182)*z(105)) - z(175)*z(10) - z(79)*z(169)*z(285) - z(79)*z(170)*z(245) - z(109)*z(169)*z(300) - z(109)*z(170)*z(263) - 0.5*Fc*z(73)*z(109)*z(291) - 0.5*Fc*z(74)*z(79)*z(292) - 0.5*Fc*z(75)*z(109)*z(292) - 0.5*Fc*z(76)*z(79)*z(293) - 0.5*Fc*z(77)*z(109)*z(293) - 0.5*Fc*z(79)*z(121)*z(294) - 0.5*Fc*z(109)*z(123)*z(294) - z(95)*(z(74)*z(79)+z(75)*z(88)) - z(98)*(z(26)*z(45)+z(31)*z(46)) - z(99)*(z(49)*z(54)+z(50)*z(60)); - z(412) = z(303)*z(201) + par.TrunkInertia*z(187)*z(306) + par.ForearmMass*(z(79)*z(314)+z(88)*z(316)+z(321)*z(94)+z(322)*z(91)) + par.UpparmMass*(z(54)*z(312)+z(60)*z(313)+z(319)*z(66)-z(320)*z(63)) + par.TrunkMass*(z(26)*z(34)+z(31)*z(311)+par.ThighLen*z(23)*z(38)-par.ThighLen*z(24)*z(34)); - z(413) = par.ThighLen*par.TrunkMass; - z(414) = z(199)*z(303) + par.TrunkInertia*z(187)*z(303) + 2*par.ForearmMass*(z(79)*z(321)+z(88)*z(322)) + 2*par.UpparmMass*(z(54)*z(319)-z(60)*z(320)) + 2*z(413)*(z(23)*z(26)-z(24)*z(31)); - z(415) = z(200)*z(303) + par.TrunkInertia*z(187)*z(324) + z(413)*z(24)*z(30) + par.ForearmMass*(z(80)*z(321)+z(89)*z(322)) - par.UpparmMass*(z(55)*z(319)+z(61)*z(320)); - z(416) = par.ForearmMass*(z(81)*z(321)+z(86)*z(322)) - z(209)*z(320); - z(417) = z(73)*z(172)*z(322) + z(74)*z(167)*z(321) + z(75)*z(167)*z(322) + z(76)*z(168)*z(321) + z(77)*z(168)*z(322) + z(79)*z(167)*z(329) + z(79)*z(168)*z(332) + z(109)*z(167)*z(341) + z(109)*z(168)*z(343) + z(121)*z(171)*z(321) + z(123)*z(171)*z(322) + 0.5*Fc*z(79)*z(345) + 0.5*Fc*z(72)*z(79)*z(339) + 0.5*Fc*z(79)*z(113)*z(346) + 0.5*Fc*z(79)*z(117)*z(347) + 0.5*Fc*z(109)*z(115)*z(346) + 0.5*Fc*z(109)*z(119)*z(347) + z(99)*(z(52)*z(320)-z(51)*z(319)-z(54)*z(330)-z(60)*z(331)) - z(166)*z(321) - z(72)*z(172)*z(321) - z(79)*z(169)*z(333) - z(79)*z(170)*z(334) - z(109)*z(169)*z(348) - z(109)*z(170)*z(349) - z(113)*z(169)*z(321) - z(115)*z(169)*z(322) - z(117)*z(170)*z(321) - z(119)*z(170)*z(322) - 0.5*Fc*z(73)*z(109)*z(339) - 0.5*Fc*z(74)*z(79)*z(340) - 0.5*Fc*z(75)*z(109)*z(340) - 0.5*Fc*z(76)*z(79)*z(342) - 0.5*Fc*z(77)*z(109)*z(342) - 0.5*Fc*z(79)*z(121)*z(344) - 0.5*Fc*z(109)*z(123)*z(344) - z(95)*(z(76)*z(321)+z(77)*z(322)+z(79)*z(332)+z(88)*z(343)) - z(98)*(z(26)*z(48)+z(31)*z(186)+par.ThighLen*z(23)*z(47)-par.ThighLen*z(24)*z(48)); - z(418) = par.ForearmMass*(z(79)*z(354)+z(88)*z(356)+z(360)*z(94)+z(361)*z(91)) + par.UpparmMass*(z(54)*z(63)+z(57)*z(66)+z(60)*z(353)+z(359)*z(63)); - z(419) = 2*par.ForearmMass*(z(79)*z(360)+z(88)*z(361)) + 2*par.UpparmMass*(z(54)*z(57)+z(60)*z(359)); - z(420) = -par.ForearmMass*(z(79)*z(363)-z(80)*z(360)-z(88)*z(364)-z(89)*z(361)) - par.UpparmMass*(z(55)*z(57)+z(39)*z(43)*z(54)-z(61)*z(359)-z(39)*z(44)*z(60)); - z(421) = z(209)*z(359) + par.ForearmMass*(z(81)*z(360)+z(86)*z(361)); - z(422) = z(73)*z(172)*z(361) + z(74)*z(167)*z(360) + z(75)*z(167)*z(361) + z(76)*z(168)*z(360) + z(77)*z(168)*z(361) + z(79)*z(123)*z(171) + z(79)*z(167)*z(368) + z(79)*z(168)*z(370) + z(109)*z(167)*z(376) + z(109)*z(168)*z(378) + z(109)*z(171)*z(380) + z(121)*z(171)*z(360) + z(123)*z(171)*z(361) + 0.5*Fc*z(79)*z(381) + 0.5*Fc*z(72)*z(79)*z(374) + 0.5*Fc*z(79)*z(113)*z(382) + 0.5*Fc*z(79)*z(117)*z(383) + 0.5*Fc*z(109)*z(115)*z(382) + 0.5*Fc*z(109)*z(119)*z(383) - z(166)*z(360) - z(72)*z(172)*z(360) - z(79)*z(169)*z(371) - z(79)*z(170)*z(372) - z(109)*z(169)*z(384) - z(109)*z(170)*z(385) - z(113)*z(169)*z(360) - z(115)*z(169)*z(361) - z(117)*z(170)*z(360) - z(119)*z(170)*z(361) - 0.5*Fc*z(73)*z(109)*z(374) - 0.5*Fc*z(74)*z(79)*z(375) - 0.5*Fc*z(75)*z(109)*z(375) - 0.5*Fc*z(76)*z(79)*z(377) - 0.5*Fc*z(77)*z(109)*z(377) - 0.5*Fc*z(79)*z(121)*z(379) - 0.5*Fc*z(109)*z(123)*z(379) - z(95)*(z(76)*z(360)+z(77)*z(361)+z(79)*z(370)+z(88)*z(378)) - z(99)*(z(51)*z(57)+z(52)*z(54)+z(52)*z(359)+z(60)*z(369)); - z(423) = par.ForearmMass*(z(79)*z(91)+z(84)*z(94)+z(88)*z(388)+z(391)*z(91)); - z(424) = par.ForearmMass*(z(79)*z(84)+z(88)*z(391)); - z(425) = par.ForearmMass*(z(79)*z(85)+z(80)*z(84)+z(88)*z(393)+z(89)*z(391)); - z(426) = par.ForearmMass*(z(81)*z(84)+z(86)*z(391)+par.UpparmLen*z(72)*z(79)-par.UpparmLen*z(73)*z(88)); - z(427) = z(72)*z(109)*z(172) + z(73)*z(79)*z(172) + z(73)*z(172)*z(391) + z(74)*z(84)*z(167) + z(75)*z(79)*z(167) + z(75)*z(167)*z(391) + z(76)*z(84)*z(168) + z(77)*z(79)*z(168) + z(77)*z(168)*z(391) + z(79)*z(123)*z(171) + z(84)*z(121)*z(171) + z(109)*z(167)*z(399) + z(109)*z(168)*z(401) + z(109)*z(171)*z(380) + z(123)*z(171)*z(391) + 0.5*Fc*z(79)*z(403) + 0.5*Fc*z(72)*z(79)*z(397) + 0.5*Fc*z(79)*z(113)*z(404) + 0.5*Fc*z(79)*z(117)*z(405) + 0.5*Fc*z(109)*z(115)*z(404) + 0.5*Fc*z(109)*z(119)*z(405) - z(84)*z(166) - z(72)*z(84)*z(172) - z(79)*z(115)*z(169) - z(79)*z(119)*z(170) - z(84)*z(113)*z(169) - z(84)*z(117)*z(170) - z(109)*z(169)*z(406) - z(109)*z(170)*z(407) - z(115)*z(169)*z(391) - z(119)*z(170)*z(391) - 0.5*Fc*z(73)*z(109)*z(397) - 0.5*Fc*z(74)*z(79)*z(398) - 0.5*Fc*z(75)*z(109)*z(398) - 0.5*Fc*z(76)*z(79)*z(400) - 0.5*Fc*z(77)*z(109)*z(400) - 0.5*Fc*z(79)*z(121)*z(402) - 0.5*Fc*z(109)*z(123)*z(402) - z(95)*(z(76)*z(84)+z(77)*z(79)+z(77)*z(391)+z(88)*z(401)); - z(428) = par.TrunkInertia*z(188)*z(236) + 2*z(46)*(z(48)+z(184))*z(201); - z(429) = par.TrunkInertia*z(188)*z(233) + 2*z(46)*z(199)*(z(48)+z(184)); - z(430) = 2*z(188)*z(239) + 2*z(46)*z(200)*(z(48)+z(184)); - z(431) = z(98)*z(30)*z(46) + z(74)*z(80)*z(168) + z(75)*z(110)*z(168) + z(80)*z(167)*z(243) + z(110)*z(167)*z(253) + 0.5*Fc*z(80)*z(259) + 0.5*Fc*z(72)*z(80)*z(251) + 0.5*Fc*z(80)*z(113)*z(260) + 0.5*Fc*z(80)*z(117)*z(261) + 0.5*Fc*z(110)*z(115)*z(260) + 0.5*Fc*z(110)*z(119)*z(261) + z(99)*(z(49)*z(55)-z(50)*z(61)) - z(80)*z(169)*z(244) - z(80)*z(170)*z(245) - z(110)*z(169)*z(262) - z(110)*z(170)*z(263) - 0.5*Fc*z(73)*z(110)*z(251) - 0.5*Fc*z(74)*z(80)*z(252) - 0.5*Fc*z(75)*z(110)*z(252) - 0.5*Fc*z(76)*z(80)*z(254) - 0.5*Fc*z(77)*z(110)*z(254) - 0.5*Fc*z(80)*z(121)*z(255) - 0.5*Fc*z(110)*z(123)*z(255) - z(95)*(z(74)*z(80)+z(75)*z(89)); - z(432) = par.TrunkInertia*z(188)*z(236) + 2*z(46)*(z(48)+z(184))*z(201) + par.ForearmMass*(z(80)*z(275)+z(89)*z(277)) - par.TrunkMass*z(30)*z(272) - par.UpparmMass*(z(55)*z(273)-z(61)*z(274)); - z(433) = par.TrunkInertia*z(188)*z(233) + par.TrunkMass*z(30)*z(267) + 2*z(46)*z(199)*(z(48)+z(184)) + par.ForearmMass*(z(80)*z(270)+z(89)*z(276)) - par.UpparmMass*(z(55)*z(268)-z(61)*z(269)); - z(434) = z(98)*z(30)*z(46) + z(74)*z(80)*z(168) + z(75)*z(110)*z(168) + z(80)*z(167)*z(243) + z(110)*z(167)*z(253) + 0.5*Fc*z(80)*z(297) + 0.5*Fc*z(72)*z(80)*z(291) + 0.5*Fc*z(80)*z(113)*z(298) + 0.5*Fc*z(80)*z(117)*z(299) + 0.5*Fc*z(110)*z(115)*z(298) + 0.5*Fc*z(110)*z(119)*z(299) + z(99)*(z(49)*z(55)-z(50)*z(61)) - z(80)*z(169)*z(285) - z(80)*z(170)*z(245) - z(110)*z(169)*z(300) - z(110)*z(170)*z(263) - 0.5*Fc*z(73)*z(110)*z(291) - 0.5*Fc*z(74)*z(80)*z(292) - 0.5*Fc*z(75)*z(110)*z(292) - 0.5*Fc*z(76)*z(80)*z(293) - 0.5*Fc*z(77)*z(110)*z(293) - 0.5*Fc*z(80)*z(121)*z(294) - 0.5*Fc*z(110)*z(123)*z(294) - z(95)*(z(74)*z(80)+z(75)*z(89)); - z(435) = z(324)*z(201) + par.TrunkInertia*z(188)*z(306) + par.ForearmMass*(z(80)*z(314)+z(89)*z(316)) - par.TrunkMass*z(30)*z(311) - par.UpparmMass*(z(55)*z(312)-z(61)*z(313)); - z(436) = z(199)*z(324) + par.TrunkInertia*z(188)*z(303) + par.ForearmMass*(z(80)*z(310)+z(89)*z(315)) - par.TrunkMass*z(30)*z(307) - par.UpparmMass*(z(55)*z(308)-z(61)*z(309)); - z(437) = z(199)*z(324) + par.TrunkInertia*z(188)*z(303) + z(413)*z(24)*z(30) + par.ForearmMass*(z(80)*z(321)+z(89)*z(322)) - par.UpparmMass*(z(55)*z(319)+z(61)*z(320)); - z(438) = z(324)*(z(200)+par.TrunkInertia*z(188)); - z(439) = z(98)*z(30)*z(186) + z(80)*z(167)*z(329) + z(80)*z(168)*z(332) + z(110)*z(167)*z(341) + z(110)*z(168)*z(343) + 0.5*Fc*z(80)*z(345) + 0.5*Fc*z(72)*z(80)*z(339) + 0.5*Fc*z(80)*z(113)*z(346) + 0.5*Fc*z(80)*z(117)*z(347) + 0.5*Fc*z(110)*z(115)*z(346) + 0.5*Fc*z(110)*z(119)*z(347) + z(99)*(z(55)*z(330)-z(61)*z(331)) - z(80)*z(169)*z(333) - z(80)*z(170)*z(334) - z(110)*z(169)*z(348) - z(110)*z(170)*z(349) - 0.5*Fc*z(73)*z(110)*z(339) - 0.5*Fc*z(74)*z(80)*z(340) - 0.5*Fc*z(75)*z(110)*z(340) - 0.5*Fc*z(76)*z(80)*z(342) - 0.5*Fc*z(77)*z(110)*z(342) - 0.5*Fc*z(80)*z(121)*z(344) - 0.5*Fc*z(110)*z(123)*z(344) - z(95)*(z(80)*z(332)+z(89)*z(343)); - z(440) = -par.ForearmMass*(z(363)*z(94)-z(80)*z(354)-z(89)*z(356)-z(364)*z(91)) - par.UpparmMass*(z(55)*z(63)+z(39)*z(43)*z(66)-z(61)*z(353)-z(39)*z(44)*z(63)); - z(441) = 2*par.UpparmMass*z(39)*(z(43)*z(55)+z(44)*z(61)) - 2*par.ForearmMass*(z(80)*z(363)-z(89)*z(364)); - z(442) = z(209)*z(39)*z(44) - par.ForearmMass*(z(81)*z(363)-z(86)*z(364)); - z(443) = z(166)*z(363) + z(72)*z(172)*z(363) + z(73)*z(172)*z(364) + z(75)*z(167)*z(364) + z(77)*z(168)*z(364) + z(80)*z(123)*z(171) + z(80)*z(167)*z(368) + z(80)*z(168)*z(370) + z(110)*z(167)*z(376) + z(110)*z(168)*z(378) + z(110)*z(171)*z(380) + z(113)*z(169)*z(363) + z(117)*z(170)*z(363) + z(123)*z(171)*z(364) + 0.5*Fc*z(80)*z(381) + 0.5*Fc*z(72)*z(80)*z(374) + 0.5*Fc*z(80)*z(113)*z(382) + 0.5*Fc*z(80)*z(117)*z(383) + 0.5*Fc*z(110)*z(115)*z(382) + 0.5*Fc*z(110)*z(119)*z(383) + z(95)*(z(76)*z(363)-z(77)*z(364)-z(80)*z(370)-z(89)*z(378)) + z(99)*(z(52)*z(55)+z(39)*z(43)*z(51)-z(61)*z(369)-z(39)*z(44)*z(52)) - z(74)*z(167)*z(363) - z(76)*z(168)*z(363) - z(80)*z(169)*z(371) - z(80)*z(170)*z(372) - z(110)*z(169)*z(384) - z(110)*z(170)*z(385) - z(115)*z(169)*z(364) - z(119)*z(170)*z(364) - z(121)*z(171)*z(363) - 0.5*Fc*z(73)*z(110)*z(374) - 0.5*Fc*z(74)*z(80)*z(375) - 0.5*Fc*z(75)*z(110)*z(375) - 0.5*Fc*z(76)*z(80)*z(377) - 0.5*Fc*z(77)*z(110)*z(377) - 0.5*Fc*z(80)*z(121)*z(379) - 0.5*Fc*z(110)*z(123)*z(379); - z(444) = par.ForearmMass*(z(80)*z(91)+z(85)*z(94)+z(89)*z(388)+z(393)*z(91)); - z(445) = par.ForearmMass*(z(80)*z(85)+z(89)*z(393)); - z(446) = par.ForearmMass*(z(81)*z(85)+z(86)*z(393)+par.UpparmLen*z(72)*z(80)-par.UpparmLen*z(73)*z(89)); - z(447) = z(72)*z(110)*z(172) + z(73)*z(80)*z(172) + z(73)*z(172)*z(393) + z(74)*z(85)*z(167) + z(75)*z(80)*z(167) + z(75)*z(167)*z(393) + z(76)*z(85)*z(168) + z(77)*z(80)*z(168) + z(77)*z(168)*z(393) + z(80)*z(123)*z(171) + z(85)*z(121)*z(171) + z(110)*z(167)*z(399) + z(110)*z(168)*z(401) + z(110)*z(171)*z(380) + z(123)*z(171)*z(393) + 0.5*Fc*z(80)*z(403) + 0.5*Fc*z(72)*z(80)*z(397) + 0.5*Fc*z(80)*z(113)*z(404) + 0.5*Fc*z(80)*z(117)*z(405) + 0.5*Fc*z(110)*z(115)*z(404) + 0.5*Fc*z(110)*z(119)*z(405) - z(85)*z(166) - z(72)*z(85)*z(172) - z(80)*z(115)*z(169) - z(80)*z(119)*z(170) - z(85)*z(113)*z(169) - z(85)*z(117)*z(170) - z(110)*z(169)*z(406) - z(110)*z(170)*z(407) - z(115)*z(169)*z(393) - z(119)*z(170)*z(393) - 0.5*Fc*z(73)*z(110)*z(397) - 0.5*Fc*z(74)*z(80)*z(398) - 0.5*Fc*z(75)*z(110)*z(398) - 0.5*Fc*z(76)*z(80)*z(400) - 0.5*Fc*z(77)*z(110)*z(400) - 0.5*Fc*z(80)*z(121)*z(402) - 0.5*Fc*z(110)*z(123)*z(402) - z(95)*(z(76)*z(85)+z(77)*z(80)+z(77)*z(393)+z(89)*z(401)); - z(448) = z(74)*z(81)*z(168) + z(75)*z(107)*z(168) + z(81)*z(167)*z(243) + z(107)*z(167)*z(253) + 0.5*Fc*z(81)*z(259) + 0.5*Fc*z(72)*z(81)*z(251) + 0.5*Fc*z(81)*z(113)*z(260) + 0.5*Fc*z(81)*z(117)*z(261) + 0.5*Fc*z(107)*z(115)*z(260) + 0.5*Fc*z(107)*z(119)*z(261) - z(178)*z(50) - z(81)*z(169)*z(244) - z(81)*z(170)*z(245) - z(107)*z(169)*z(262) - z(107)*z(170)*z(263) - 0.5*Fc*z(73)*z(107)*z(251) - 0.5*Fc*z(74)*z(81)*z(252) - 0.5*Fc*z(75)*z(107)*z(252) - 0.5*Fc*z(76)*z(81)*z(254) - 0.5*Fc*z(77)*z(107)*z(254) - 0.5*Fc*z(81)*z(121)*z(255) - 0.5*Fc*z(107)*z(123)*z(255) - z(95)*(z(74)*z(81)+z(75)*z(86)); - z(449) = z(209)*z(274) + par.ForearmMass*(z(81)*z(275)+z(86)*z(277)); - z(450) = z(74)*z(81)*z(168) + z(75)*z(107)*z(168) + z(81)*z(167)*z(243) + z(107)*z(167)*z(253) + 0.5*Fc*z(81)*z(297) + 0.5*Fc*z(72)*z(81)*z(291) + 0.5*Fc*z(81)*z(113)*z(298) + 0.5*Fc*z(81)*z(117)*z(299) + 0.5*Fc*z(107)*z(115)*z(298) + 0.5*Fc*z(107)*z(119)*z(299) - z(178)*z(50) - z(81)*z(169)*z(285) - z(81)*z(170)*z(245) - z(107)*z(169)*z(300) - z(107)*z(170)*z(263) - 0.5*Fc*z(73)*z(107)*z(291) - 0.5*Fc*z(74)*z(81)*z(292) - 0.5*Fc*z(75)*z(107)*z(292) - 0.5*Fc*z(76)*z(81)*z(293) - 0.5*Fc*z(77)*z(107)*z(293) - 0.5*Fc*z(81)*z(121)*z(294) - 0.5*Fc*z(107)*z(123)*z(294) - z(95)*(z(74)*z(81)+z(75)*z(86)); - z(451) = z(209)*z(313) + par.ForearmMass*(z(81)*z(314)+z(86)*z(316)); - z(452) = z(81)*z(167)*z(329) + z(81)*z(168)*z(332) + z(107)*z(167)*z(341) + z(107)*z(168)*z(343) + 0.5*Fc*z(81)*z(345) + 0.5*Fc*z(72)*z(81)*z(339) + 0.5*Fc*z(81)*z(113)*z(346) + 0.5*Fc*z(81)*z(117)*z(347) + 0.5*Fc*z(107)*z(115)*z(346) + 0.5*Fc*z(107)*z(119)*z(347) - z(178)*z(331) - z(81)*z(169)*z(333) - z(81)*z(170)*z(334) - z(107)*z(169)*z(348) - z(107)*z(170)*z(349) - 0.5*Fc*z(73)*z(107)*z(339) - 0.5*Fc*z(74)*z(81)*z(340) - 0.5*Fc*z(75)*z(107)*z(340) - 0.5*Fc*z(76)*z(81)*z(342) - 0.5*Fc*z(77)*z(107)*z(342) - 0.5*Fc*z(81)*z(121)*z(344) - 0.5*Fc*z(107)*z(123)*z(344) - z(95)*(z(81)*z(332)+z(86)*z(343)); - z(453) = z(209)*z(353) + par.ForearmMass*(z(81)*z(354)+z(86)*z(356)); - z(454) = z(81)*z(123)*z(171) + z(81)*z(167)*z(368) + z(81)*z(168)*z(370) + z(107)*z(167)*z(376) + z(107)*z(168)*z(378) + z(107)*z(171)*z(380) + 0.5*Fc*z(81)*z(381) + 0.5*Fc*z(72)*z(81)*z(374) + 0.5*Fc*z(81)*z(113)*z(382) + 0.5*Fc*z(81)*z(117)*z(383) + 0.5*Fc*z(107)*z(115)*z(382) + 0.5*Fc*z(107)*z(119)*z(383) - z(178)*z(369) - z(81)*z(169)*z(371) - z(81)*z(170)*z(372) - z(107)*z(169)*z(384) - z(107)*z(170)*z(385) - 0.5*Fc*z(73)*z(107)*z(374) - 0.5*Fc*z(74)*z(81)*z(375) - 0.5*Fc*z(75)*z(107)*z(375) - 0.5*Fc*z(76)*z(81)*z(377) - 0.5*Fc*z(77)*z(107)*z(377) - 0.5*Fc*z(81)*z(121)*z(379) - 0.5*Fc*z(107)*z(123)*z(379) - z(95)*(z(81)*z(370)+z(86)*z(378)); - z(455) = par.ForearmMass*(z(81)*z(91)+z(86)*z(388)+par.UpparmLen*z(72)*z(94)-par.UpparmLen*z(73)*z(91)); - z(456) = par.ForearmMass*par.UpparmLen; - z(457) = z(456)*(z(72)*z(81)-z(73)*z(86)); - z(458) = z(72)*z(107)*z(172) + z(73)*z(81)*z(172) + z(75)*z(81)*z(167) + z(77)*z(81)*z(168) + z(81)*z(123)*z(171) + z(107)*z(167)*z(399) + z(107)*z(168)*z(401) + z(107)*z(171)*z(380) + 0.5*Fc*z(81)*z(403) + par.UpparmLen*z(72)*z(74)*z(167) + par.UpparmLen*z(72)*z(76)*z(168) + par.UpparmLen*z(72)*z(121)*z(171) + par.UpparmLen*z(73)*z(115)*z(169) + par.UpparmLen*z(73)*z(119)*z(170) + 0.5*Fc*z(72)*z(81)*z(397) + 0.5*Fc*z(81)*z(113)*z(404) + 0.5*Fc*z(81)*z(117)*z(405) + 0.5*Fc*z(107)*z(115)*z(404) + 0.5*Fc*z(107)*z(119)*z(405) - par.UpparmLen*z(72)*z(166) - z(81)*z(115)*z(169) - z(81)*z(119)*z(170) - z(107)*z(169)*z(406) - z(107)*z(170)*z(407) - par.UpparmLen*z(72)*z(113)*z(169) - par.UpparmLen*z(72)*z(117)*z(170) - par.UpparmLen*z(73)*z(75)*z(167) - par.UpparmLen*z(73)*z(77)*z(168) - par.UpparmLen*z(73)*z(123)*z(171) - par.UpparmLen*z(172)*z(72)^2 - par.UpparmLen*z(172)*z(73)^2 - 0.5*Fc*z(73)*z(107)*z(397) - 0.5*Fc*z(74)*z(81)*z(398) - 0.5*Fc*z(75)*z(107)*z(398) - 0.5*Fc*z(76)*z(81)*z(400) - 0.5*Fc*z(77)*z(107)*z(400) - 0.5*Fc*z(81)*z(121)*z(402) - 0.5*Fc*z(107)*z(123)*z(402) - z(95)*(z(77)*z(81)+z(86)*z(401)+par.UpparmLen*z(72)*z(76)-par.UpparmLen*z(73)*z(77)); - z(459) = 0.5*par.ForearmLen*(2*z(75)*z(168)+2*z(167)*z(253)+Fc*z(115)*z(260)+Fc*z(119)*z(261)-2*z(169)*z(262)-2*z(170)*z(263)-Fc*z(73)*z(251)-Fc*z(75)*z(252)-Fc*z(77)*z(254)-Fc*z(123)*z(255)) - z(180)*z(75); - z(460) = 0.5*par.ForearmLen*(2*z(75)*z(168)+2*z(167)*z(253)+Fc*z(115)*z(298)+Fc*z(119)*z(299)-2*z(169)*z(300)-2*z(170)*z(263)-Fc*z(73)*z(291)-Fc*z(75)*z(292)-Fc*z(77)*z(293)-Fc*z(123)*z(294)) - z(180)*z(75); - z(461) = 0.5*par.ForearmLen*(2*z(167)*z(341)+2*z(168)*z(343)+Fc*z(115)*z(346)+Fc*z(119)*z(347)-2*z(169)*z(348)-2*z(170)*z(349)-Fc*z(73)*z(339)-Fc*z(75)*z(340)-Fc*z(77)*z(342)-Fc*z(123)*z(344)) - z(180)*z(343); - z(462) = 0.5*par.ForearmLen*(2*z(167)*z(376)+2*z(168)*z(378)+2*z(171)*z(380)+Fc*z(115)*z(382)+Fc*z(119)*z(383)-2*z(169)*z(384)-2*z(170)*z(385)-Fc*z(73)*z(374)-Fc*z(75)*z(375)-Fc*z(77)*z(377)-Fc*z(123)*z(379)) - z(180)*z(378); - z(463) = 0.5*par.ForearmLen*(2*z(72)*z(172)+2*z(167)*z(399)+2*z(168)*z(401)+2*z(171)*z(380)+Fc*z(115)*z(404)+Fc*z(119)*z(405)-2*z(169)*z(406)-2*z(170)*z(407)-Fc*z(73)*z(397)-Fc*z(75)*z(398)-Fc*z(77)*z(400)-Fc*z(123)*z(402)) - z(180)*z(401); - z(464) = (z(137)*z(244)+z(138)*z(245)+z(141)*z(246)+z(142)*z(248)+z(143)*z(183)+z(144)*z(45)+z(149)*z(2)-z(146)*z(243)-z(147)*z(74)-z(148)*z(249)-z(150)*z(1)-z(152)*z(10)-z(153)*z(9)-z(155)*z(241)-z(156)*z(49))/(z(136)+2*z(137)*z(113)+2*z(138)*z(117)+2*z(139)*z(72)+2*z(140)*z(7)+2*z(141)*z(128)+2*z(142)*z(132)+2*z(143)*z(45)+2*z(144)*z(47)-2*z(145)*z(121)-2*z(146)*z(74)-2*z(147)*z(76)-2*z(148)*z(124)-2*z(149)*z(1)-2*z(150)*z(2)-2*z(151)*z(23)-2*z(152)*z(9)-2*z(153)*z(11)-2*z(154)*z(43)-2*z(155)*z(49)-2*z(156)*z(51))^0.5; - z(465) = (z(137)*z(285)+z(138)*z(245)+z(141)*z(286)+z(142)*z(288)+z(143)*z(183)+z(144)*z(45)-z(140)*z(8)-z(146)*z(243)-z(147)*z(74)-z(148)*z(289)-z(152)*z(10)-z(153)*z(9)-z(155)*z(241)-z(156)*z(49))/(z(136)+2*z(137)*z(113)+2*z(138)*z(117)+2*z(139)*z(72)+2*z(140)*z(7)+2*z(141)*z(128)+2*z(142)*z(132)+2*z(143)*z(45)+2*z(144)*z(47)-2*z(145)*z(121)-2*z(146)*z(74)-2*z(147)*z(76)-2*z(148)*z(124)-2*z(149)*z(1)-2*z(150)*z(2)-2*z(151)*z(23)-2*z(152)*z(9)-2*z(153)*z(11)-2*z(154)*z(43)-2*z(155)*z(49)-2*z(156)*z(51))^0.5; - z(466) = (z(137)*z(333)+z(138)*z(334)+z(141)*z(335)+z(142)*z(337)+z(143)*z(46)+z(144)*z(48)+z(151)*z(24)-z(146)*z(329)-z(147)*z(332)-z(148)*z(125)-z(155)*z(327)-z(156)*z(330))/(z(136)+2*z(137)*z(113)+2*z(138)*z(117)+2*z(139)*z(72)+2*z(140)*z(7)+2*z(141)*z(128)+2*z(142)*z(132)+2*z(143)*z(45)+2*z(144)*z(47)-2*z(145)*z(121)-2*z(146)*z(74)-2*z(147)*z(76)-2*z(148)*z(124)-2*z(149)*z(1)-2*z(150)*z(2)-2*z(151)*z(23)-2*z(152)*z(9)-2*z(153)*z(11)-2*z(154)*z(43)-2*z(155)*z(49)-2*z(156)*z(51))^0.5; - z(467) = (z(137)*z(371)+z(138)*z(372)+z(141)*z(129)+z(142)*z(133)+z(154)*z(44)-z(145)*z(123)-z(146)*z(368)-z(147)*z(370)-z(155)*z(50)-z(156)*z(52))/(z(136)+2*z(137)*z(113)+2*z(138)*z(117)+2*z(139)*z(72)+2*z(140)*z(7)+2*z(141)*z(128)+2*z(142)*z(132)+2*z(143)*z(45)+2*z(144)*z(47)-2*z(145)*z(121)-2*z(146)*z(74)-2*z(147)*z(76)-2*z(148)*z(124)-2*z(149)*z(1)-2*z(150)*z(2)-2*z(151)*z(23)-2*z(152)*z(9)-2*z(153)*z(11)-2*z(154)*z(43)-2*z(155)*z(49)-2*z(156)*z(51))^0.5; - z(468) = par.ForearmLen*(par.ShankLen*z(115)+par.ThighLen*z(119)-par.TrunkLen*z(123)-par.UpparmLen*z(73)-z(111)*z(75)-z(112)*z(77))/(z(136)+2*z(137)*z(113)+2*z(138)*z(117)+2*z(139)*z(72)+2*z(140)*z(7)+2*z(141)*z(128)+2*z(142)*z(132)+2*z(143)*z(45)+2*z(144)*z(47)-2*z(145)*z(121)-2*z(146)*z(74)-2*z(147)*z(76)-2*z(148)*z(124)-2*z(149)*z(1)-2*z(150)*z(2)-2*z(151)*z(23)-2*z(152)*z(9)-2*z(153)*z(11)-2*z(154)*z(43)-2*z(155)*z(49)-2*z(156)*z(51))^0.5; - z(469) = z(237) + z(238)*q1pp + z(238)*q2pp + z(240)*q3pp - z(265); - z(470) = z(278) + z(280)*q1pp + z(282)*q2pp + z(283)*q3pp + z(284)*q4pp + z(211)*z(276)*q5pp - z(302); - z(471) = z(317) + z(318)*q1pp + z(323)*q2pp + z(325)*q3pp + z(326)*q4pp + z(211)*z(315)*q5pp - z(350); - z(472) = z(357) + z(358)*q1pp + z(362)*q2pp + z(365)*q3pp + z(366)*q4pp + z(211)*z(355)*q5pp - z(386); - z(473) = z(389) + z(392)*q2pp + z(394)*q3pp + z(395)*q4pp + 2*z(390)*q1pp + z(211)*z(387)*q5pp - z(408); - z(474) = z(237) + z(238)*q1pp + z(238)*q2pp + z(240)*q3pp - z(409); - z(475) = z(410) + z(238)*q2pp + z(240)*q3pp + z(282)*q1pp - z(411); - z(476) = z(412) + z(323)*q1pp + z(414)*q2pp + z(415)*q3pp + z(416)*q4pp + z(211)*z(322)*q5pp - z(417); - z(477) = z(418) + z(362)*q1pp + z(419)*q2pp + z(420)*q3pp + z(421)*q4pp + z(211)*z(361)*q5pp - z(422); - z(478) = z(423) + z(392)*q1pp + z(425)*q3pp + z(426)*q4pp + 2*z(424)*q2pp + z(211)*z(391)*q5pp - z(427); - z(479) = z(428) + z(429)*q1pp + z(429)*q2pp + z(430)*q3pp - z(431); - z(480) = z(432) + z(429)*q2pp + z(430)*q3pp + z(433)*q1pp - z(434); - z(481) = z(435) + z(436)*q1pp + z(437)*q2pp + z(438)*q3pp - z(439); - z(482) = z(440) + z(365)*q1pp + z(420)*q2pp + z(441)*q3pp + z(442)*q4pp + z(211)*z(364)*q5pp - z(443); - z(483) = z(444) + z(394)*q1pp + z(425)*q2pp + z(446)*q4pp + 2*z(445)*q3pp + z(211)*z(393)*q5pp - z(447); - z(484) = z(449) + z(284)*q1pp - z(450); - z(485) = z(451) + z(326)*q1pp + z(416)*q2pp - z(452); - z(486) = z(453) + z(366)*q1pp + z(421)*q2pp + z(442)*q3pp - z(454); - z(487) = par.UpparmLen*z(211); - z(488) = z(455) + z(395)*q1pp + z(426)*q2pp + z(446)*q3pp + 2*z(457)*q4pp - z(458) - z(487)*z(73)*q5pp; - z(489) = z(211)*z(277) + z(211)*z(276)*q1pp - z(460); - z(490) = z(211)*z(316) + z(211)*z(315)*q1pp + z(211)*z(322)*q2pp - z(461); - z(491) = z(211)*z(356) + z(211)*z(355)*q1pp + z(211)*z(361)*q2pp + z(211)*z(364)*q3pp - z(462); - z(492) = z(211)*z(388) + z(211)*z(387)*q1pp + z(211)*z(391)*q2pp + z(211)*z(393)*q3pp - z(463) - z(487)*z(73)*q4pp; - z(493) = z(45)*z(46)*q1p + z(45)*z(46)*q2p + z(46)*z(232)*q1p + z(46)*z(232)*q2p + z(48)*z(183)*q1p + z(48)*z(183)*q2p + z(183)*z(184)*q1p + z(183)*z(184)*q2p + 2*z(46)*z(48)*q3p + 2*z(46)*z(184)*q3p + z(45)*z(194) + z(46)*z(197) + z(48)*z(192) + z(183)*z(195); + z(333) = -z(23)*z(22) - z(24)*z(16); + z(334) = z(43)*z(34) + z(44)*z(333); + z(335) = z(43)*z(333) - z(44)*z(34); + z(336) = z(72)*z(334) + z(73)*z(335); + z(337) = z(72)*z(331) - z(73)*z(330); + z(338) = z(72)*z(335) - z(73)*z(334); + z(339) = z(325)*z(237) + par.TrunkInertia*z(223)*z(328) + par.ForearmMass*(z(78)*z(336)+z(87)*z(338)+z(332)*z(94)+z(337)*z(91)) + par.TrunkMass*(z(25)*z(34)+z(28)*z(38)+z(32)*z(333)+z(329)*z(34)) + par.UpparmMass*(z(53)*z(334)+z(59)*z(335)+z(330)*z(66)+z(331)*z(63)); + z(340) = z(235)*z(325) + par.TrunkInertia*z(223)*z(325) + 2*par.ForearmMass*(z(78)*z(332)+z(87)*z(337)) + 2*par.TrunkMass*(z(25)*z(28)+z(32)*z(329)) + 2*par.UpparmMass*(z(53)*z(330)+z(59)*z(331)); + z(341) = par.ThighLen*(z(23)*z(43)-z(24)*z(44)); + z(342) = par.ThighLen*(z(23)*z(44)+z(24)*z(43)); + z(343) = z(72)*z(341) - z(73)*z(342); + z(344) = -z(72)*z(342) - z(73)*z(341); + z(345) = z(235)*z(325) + par.TrunkInertia*z(223)*z(325) + par.ForearmMass*(z(78)*z(343)+z(79)*z(332)+z(87)*z(344)+z(88)*z(337)) + par.TrunkMass*(z(26)*z(28)+z(31)*z(329)+par.ThighLen*z(23)*z(25)-par.ThighLen*z(24)*z(32)) - par.UpparmMass*(z(59)*z(342)-z(53)*z(341)-z(54)*z(330)-z(60)*z(331)); + z(346) = 2*z(46)*z(139) + 2*z(48)*z(148); + z(347) = z(236)*z(325) + par.TrunkInertia*z(223)*z(346) + par.ForearmMass*(z(80)*z(332)+z(89)*z(337)) - par.TrunkMass*z(30)*z(329) - par.UpparmMass*(z(55)*z(330)-z(61)*z(331)); + z(348) = z(245)*z(331) + par.ForearmMass*(z(81)*z(332)+z(86)*z(337)); + z(349) = 2*z(172)*z(156) + 2*z(173)*z(160) + 2*z(174)*z(162) + 2*z(176)*z(169) + 2*z(177)*z(46) + 2*z(178)*z(48) + 2*z(191)*z(24) - 2*z(179)*z(146) - 2*z(180)*z(153) - 2*z(181)*z(125) - 2*z(185)*z(140) - 2*z(186)*z(149); + z(350) = par.UpparmLen*z(349)/(z(198)^0.5*z(199)^2); + z(351) = z(111)*z(349)/(z(198)^0.5*z(199)^2); + z(352) = z(72)*z(143) - z(73)*z(140); + z(353) = z(112)*z(349)/(z(198)^0.5*z(199)^2); + z(354) = z(72)*z(150) - z(73)*z(149); + z(355) = par.TrunkLen*z(349)/(z(198)^0.5*z(199)^2); + z(356) = par.ForearmLen*z(349)/(z(198)^0.5*z(199)^2); + z(357) = par.ShankLen*z(349)/(z(198)^0.5*z(199)^2); + z(358) = par.ThighLen*z(349)/(z(198)^0.5*z(199)^2); + z(359) = z(1)*z(352) + z(2)*z(354); + z(360) = z(9)*z(352) + z(11)*z(354); + z(361) = z(73)*z(213)*z(337) + z(74)*z(208)*z(332) + z(75)*z(208)*z(337) + z(76)*z(209)*z(332) + z(77)*z(209)*z(337) + z(78)*z(146)*z(208) + z(78)*z(153)*z(209) + z(108)*z(208)*z(352) + z(108)*z(209)*z(354) + z(121)*z(212)*z(332) + z(123)*z(212)*z(337) + 0.5*Fc*z(78)*z(356) + 0.5*Fc*z(72)*z(78)*z(350) + 0.5*Fc*z(78)*z(113)*z(357) + 0.5*Fc*z(78)*z(117)*z(358) + 0.5*Fc*z(108)*z(115)*z(357) + 0.5*Fc*z(108)*z(119)*z(358) - z(207)*z(332) - z(72)*z(213)*z(332) - z(78)*z(156)*z(210) - z(78)*z(160)*z(211) - z(108)*z(210)*z(359) - z(108)*z(211)*z(360) - z(113)*z(210)*z(332) - z(115)*z(210)*z(337) - z(117)*z(211)*z(332) - z(119)*z(211)*z(337) - 0.5*Fc*z(73)*z(108)*z(350) - 0.5*Fc*z(74)*z(78)*z(351) - 0.5*Fc*z(75)*z(108)*z(351) - 0.5*Fc*z(76)*z(78)*z(353) - 0.5*Fc*z(77)*z(108)*z(353) - 0.5*Fc*z(78)*z(121)*z(355) - 0.5*Fc*z(108)*z(123)*z(355) - z(95)*(z(76)*z(332)+z(77)*z(337)+z(78)*z(153)+z(87)*z(354)) - z(98)*(z(25)*z(48)+z(28)*z(47)+z(32)*z(148)+z(48)*z(329)) - z(99)*(z(51)*z(330)+z(52)*z(331)+z(53)*z(149)+z(59)*z(150)); + z(362) = -z(25)*z(43) - z(41)*z(44); + z(363) = z(56)*z(72) + z(73)*z(362); + z(364) = -z(43)*z(42) - z(44)*z(34); + z(365) = z(72)*z(63) + z(73)*z(364); + z(366) = z(72)*z(362) - z(56)*z(73); + z(367) = z(72)*z(364) - z(73)*z(63); + z(368) = par.ForearmMass*(z(78)*z(365)+z(87)*z(367)+z(363)*z(94)+z(366)*z(91)) + par.UpparmMass*(z(53)*z(63)+z(56)*z(66)+z(59)*z(364)+z(362)*z(63)); + z(369) = 2*par.ForearmMass*(z(78)*z(363)+z(87)*z(366)) + 2*par.UpparmMass*(z(53)*z(56)+z(59)*z(362)); + z(370) = -z(26)*z(43) - z(40)*z(44); + z(371) = z(57)*z(72) + z(73)*z(370); + z(372) = z(72)*z(370) - z(57)*z(73); + z(373) = par.ForearmMass*(z(78)*z(371)+z(79)*z(363)+z(87)*z(372)+z(88)*z(366)) + par.UpparmMass*(z(53)*z(57)+z(54)*z(56)+z(59)*z(370)+z(60)*z(362)); + z(374) = z(39)*(z(43)*z(72)-z(44)*z(73)); + z(375) = z(39)*(z(43)*z(73)+z(44)*z(72)); + z(376) = -par.ForearmMass*(z(78)*z(374)-z(80)*z(363)-z(87)*z(375)-z(89)*z(366)) - par.UpparmMass*(z(55)*z(56)+z(39)*z(43)*z(53)-z(61)*z(362)-z(39)*z(44)*z(59)); + z(377) = z(245)*z(362) + par.ForearmMass*(z(81)*z(363)+z(86)*z(366)); + z(378) = 2*z(172)*z(155) + 2*z(173)*z(159) + 2*z(174)*z(129) + 2*z(176)*z(133) + 2*z(192)*z(44) - 2*z(179)*z(145) - 2*z(180)*z(152) - 2*z(185)*z(50) - 2*z(186)*z(52) - 2*z(190)*z(123); + z(379) = par.UpparmLen*z(378)/(z(198)^0.5*z(199)^2); + z(380) = z(111)*z(378)/(z(198)^0.5*z(199)^2); + z(381) = z(72)*z(144) - z(50)*z(73); + z(382) = z(112)*z(378)/(z(198)^0.5*z(199)^2); + z(383) = z(72)*z(151) - z(52)*z(73); + z(384) = par.TrunkLen*z(378)/(z(198)^0.5*z(199)^2); + z(385) = z(44)*z(73) - z(43)*z(72); + z(386) = par.ForearmLen*z(378)/(z(198)^0.5*z(199)^2); + z(387) = par.ShankLen*z(378)/(z(198)^0.5*z(199)^2); + z(388) = par.ThighLen*z(378)/(z(198)^0.5*z(199)^2); + z(389) = z(1)*z(381) + z(2)*z(383); + z(390) = z(9)*z(381) + z(11)*z(383); + z(391) = z(73)*z(213)*z(366) + z(74)*z(208)*z(363) + z(75)*z(208)*z(366) + z(76)*z(209)*z(363) + z(77)*z(209)*z(366) + z(78)*z(123)*z(212) + z(78)*z(145)*z(208) + z(78)*z(152)*z(209) + z(108)*z(208)*z(381) + z(108)*z(209)*z(383) + z(108)*z(212)*z(385) + z(121)*z(212)*z(363) + z(123)*z(212)*z(366) + 0.5*Fc*z(78)*z(386) + 0.5*Fc*z(72)*z(78)*z(379) + 0.5*Fc*z(78)*z(113)*z(387) + 0.5*Fc*z(78)*z(117)*z(388) + 0.5*Fc*z(108)*z(115)*z(387) + 0.5*Fc*z(108)*z(119)*z(388) - z(207)*z(363) - z(72)*z(213)*z(363) - z(78)*z(155)*z(210) - z(78)*z(159)*z(211) - z(108)*z(210)*z(389) - z(108)*z(211)*z(390) - z(113)*z(210)*z(363) - z(115)*z(210)*z(366) - z(117)*z(211)*z(363) - z(119)*z(211)*z(366) - 0.5*Fc*z(73)*z(108)*z(379) - 0.5*Fc*z(74)*z(78)*z(380) - 0.5*Fc*z(75)*z(108)*z(380) - 0.5*Fc*z(76)*z(78)*z(382) - 0.5*Fc*z(77)*z(108)*z(382) - 0.5*Fc*z(78)*z(121)*z(384) - 0.5*Fc*z(108)*z(123)*z(384) - z(95)*(z(76)*z(363)+z(77)*z(366)+z(78)*z(152)+z(87)*z(383)) - z(99)*(z(51)*z(56)+z(52)*z(53)+z(52)*z(362)+z(59)*z(151)); + z(392) = -z(53)*z(72) - z(67)*z(73); + z(393) = -z(72)*z(71) - z(73)*z(63); + z(394) = par.ForearmMass*(z(78)*z(91)+z(83)*z(94)+z(87)*z(393)+z(392)*z(91)); + z(395) = par.ForearmMass*(z(78)*z(83)+z(87)*z(392)); + z(396) = -z(54)*z(72) - z(68)*z(73); + z(397) = par.ForearmMass*(z(78)*z(84)+z(79)*z(83)+z(87)*z(396)+z(88)*z(392)); + z(398) = z(55)*z(72) - z(69)*z(73); + z(399) = par.ForearmMass*(z(78)*z(85)+z(80)*z(83)+z(87)*z(398)+z(89)*z(392)); + z(400) = par.ForearmMass*(z(81)*z(83)+z(86)*z(392)+par.UpparmLen*z(72)*z(78)-par.UpparmLen*z(73)*z(87)); + z(401) = 2*z(172)*z(115) + 2*z(173)*z(119) - 2*z(179)*z(75) - 2*z(180)*z(77) - 2*z(188)*z(73) - 2*z(190)*z(123); + z(402) = par.UpparmLen*z(401)/(z(198)^0.5*z(199)^2); + z(403) = z(111)*z(401)/(z(198)^0.5*z(199)^2); + z(404) = -z(49)*z(72) - z(50)*z(73); + z(405) = z(112)*z(401)/(z(198)^0.5*z(199)^2); + z(406) = -z(51)*z(72) - z(52)*z(73); + z(407) = par.TrunkLen*z(401)/(z(198)^0.5*z(199)^2); + z(408) = par.ForearmLen*z(401)/(z(198)^0.5*z(199)^2); + z(409) = par.ShankLen*z(401)/(z(198)^0.5*z(199)^2); + z(410) = par.ThighLen*z(401)/(z(198)^0.5*z(199)^2); + z(411) = z(1)*z(404) + z(2)*z(406); + z(412) = z(9)*z(404) + z(11)*z(406); + z(413) = z(72)*z(108)*z(213) + z(73)*z(78)*z(213) + z(73)*z(213)*z(392) + z(74)*z(83)*z(208) + z(75)*z(78)*z(208) + z(75)*z(208)*z(392) + z(76)*z(83)*z(209) + z(77)*z(78)*z(209) + z(77)*z(209)*z(392) + z(78)*z(123)*z(212) + z(83)*z(121)*z(212) + z(108)*z(208)*z(404) + z(108)*z(209)*z(406) + z(108)*z(212)*z(385) + z(123)*z(212)*z(392) + 0.5*Fc*z(78)*z(408) + 0.5*Fc*z(72)*z(78)*z(402) + 0.5*Fc*z(78)*z(113)*z(409) + 0.5*Fc*z(78)*z(117)*z(410) + 0.5*Fc*z(108)*z(115)*z(409) + 0.5*Fc*z(108)*z(119)*z(410) - z(83)*z(207) - z(72)*z(83)*z(213) - z(78)*z(115)*z(210) - z(78)*z(119)*z(211) - z(83)*z(113)*z(210) - z(83)*z(117)*z(211) - z(108)*z(210)*z(411) - z(108)*z(211)*z(412) - z(115)*z(210)*z(392) - z(119)*z(211)*z(392) - 0.5*Fc*z(73)*z(108)*z(402) - 0.5*Fc*z(74)*z(78)*z(403) - 0.5*Fc*z(75)*z(108)*z(403) - 0.5*Fc*z(76)*z(78)*z(405) - 0.5*Fc*z(77)*z(108)*z(405) - 0.5*Fc*z(78)*z(121)*z(407) - 0.5*Fc*z(108)*z(123)*z(407) - z(95)*(z(76)*z(83)+z(77)*z(78)+z(77)*z(392)+z(87)*z(406)); + z(414) = z(74)*z(79)*z(209) + z(75)*z(109)*z(209) + z(79)*z(147)*z(208) + z(109)*z(208)*z(280) + 0.5*Fc*z(79)*z(286) + 0.5*Fc*z(72)*z(79)*z(278) + 0.5*Fc*z(79)*z(113)*z(287) + 0.5*Fc*z(79)*z(117)*z(288) + 0.5*Fc*z(109)*z(115)*z(287) + 0.5*Fc*z(109)*z(119)*z(288) + par.ThighLen*(z(10)*z(106)+z(10)*z(291)-z(9)*z(285)-z(136)*z(105)) - z(216)*z(10) - z(79)*z(154)*z(210) - z(79)*z(158)*z(211) - z(109)*z(210)*z(289) - z(109)*z(211)*z(290) - 0.5*Fc*z(73)*z(109)*z(278) - 0.5*Fc*z(74)*z(79)*z(279) - 0.5*Fc*z(75)*z(109)*z(279) - 0.5*Fc*z(76)*z(79)*z(281) - 0.5*Fc*z(77)*z(109)*z(281) - 0.5*Fc*z(79)*z(121)*z(282) - 0.5*Fc*z(109)*z(123)*z(282) - z(95)*(z(74)*z(79)+z(75)*z(88)) - z(98)*(z(26)*z(45)+z(31)*z(46)) - z(99)*(z(49)*z(54)+z(50)*z(60)); + z(415) = z(269)*z(237) + par.TrunkInertia*z(223)*z(272) + z(241)*z(7)*z(6) + par.ForearmMass*(z(79)*z(302)+z(88)*z(304)) + par.TrunkMass*(z(26)*z(298)+z(31)*z(299)) + par.UpparmMass*(z(54)*z(300)+z(60)*z(301)); + z(416) = z(74)*z(79)*z(209) + z(75)*z(109)*z(209) + z(79)*z(147)*z(208) + z(109)*z(208)*z(280) + 0.5*Fc*z(79)*z(319) + 0.5*Fc*z(72)*z(79)*z(313) + 0.5*Fc*z(79)*z(113)*z(320) + 0.5*Fc*z(79)*z(117)*z(321) + 0.5*Fc*z(109)*z(115)*z(320) + 0.5*Fc*z(109)*z(119)*z(321) + par.ThighLen*(z(10)*z(106)+z(10)*z(323)-z(9)*z(318)-z(136)*z(105)) - z(216)*z(10) - z(79)*z(157)*z(210) - z(79)*z(158)*z(211) - z(109)*z(210)*z(322) - z(109)*z(211)*z(290) - 0.5*Fc*z(73)*z(109)*z(313) - 0.5*Fc*z(74)*z(79)*z(314) - 0.5*Fc*z(75)*z(109)*z(314) - 0.5*Fc*z(76)*z(79)*z(315) - 0.5*Fc*z(77)*z(109)*z(315) - 0.5*Fc*z(79)*z(121)*z(316) - 0.5*Fc*z(109)*z(123)*z(316) - z(95)*(z(74)*z(79)+z(75)*z(88)) - z(98)*(z(26)*z(45)+z(31)*z(46)) - z(99)*(z(49)*z(54)+z(50)*z(60)); + z(417) = z(325)*z(237) + par.TrunkInertia*z(223)*z(328) + par.ForearmMass*(z(79)*z(336)+z(88)*z(338)+z(343)*z(94)+z(344)*z(91)) + par.UpparmMass*(z(54)*z(334)+z(60)*z(335)+z(341)*z(66)-z(342)*z(63)) + par.TrunkMass*(z(26)*z(34)+z(31)*z(333)+par.ThighLen*z(23)*z(38)-par.ThighLen*z(24)*z(34)); + z(418) = par.ThighLen*par.TrunkMass; + z(419) = z(235)*z(325) + par.TrunkInertia*z(223)*z(325) + 2*par.ForearmMass*(z(79)*z(343)+z(88)*z(344)) + 2*par.UpparmMass*(z(54)*z(341)-z(60)*z(342)) + 2*z(418)*(z(23)*z(26)-z(24)*z(31)); + z(420) = z(236)*z(325) + par.TrunkInertia*z(223)*z(346) + z(418)*z(24)*z(30) + par.ForearmMass*(z(80)*z(343)+z(89)*z(344)) - par.UpparmMass*(z(55)*z(341)+z(61)*z(342)); + z(421) = par.ForearmMass*(z(81)*z(343)+z(86)*z(344)) - z(245)*z(342); + z(422) = z(73)*z(213)*z(344) + z(74)*z(208)*z(343) + z(75)*z(208)*z(344) + z(76)*z(209)*z(343) + z(77)*z(209)*z(344) + z(79)*z(146)*z(208) + z(79)*z(153)*z(209) + z(109)*z(208)*z(352) + z(109)*z(209)*z(354) + z(121)*z(212)*z(343) + z(123)*z(212)*z(344) + 0.5*Fc*z(79)*z(356) + 0.5*Fc*z(72)*z(79)*z(350) + 0.5*Fc*z(79)*z(113)*z(357) + 0.5*Fc*z(79)*z(117)*z(358) + 0.5*Fc*z(109)*z(115)*z(357) + 0.5*Fc*z(109)*z(119)*z(358) + z(99)*(z(52)*z(342)-z(51)*z(341)-z(54)*z(149)-z(60)*z(150)) - z(207)*z(343) - z(72)*z(213)*z(343) - z(79)*z(156)*z(210) - z(79)*z(160)*z(211) - z(109)*z(210)*z(359) - z(109)*z(211)*z(360) - z(113)*z(210)*z(343) - z(115)*z(210)*z(344) - z(117)*z(211)*z(343) - z(119)*z(211)*z(344) - 0.5*Fc*z(73)*z(109)*z(350) - 0.5*Fc*z(74)*z(79)*z(351) - 0.5*Fc*z(75)*z(109)*z(351) - 0.5*Fc*z(76)*z(79)*z(353) - 0.5*Fc*z(77)*z(109)*z(353) - 0.5*Fc*z(79)*z(121)*z(355) - 0.5*Fc*z(109)*z(123)*z(355) - z(95)*(z(76)*z(343)+z(77)*z(344)+z(79)*z(153)+z(88)*z(354)) - z(98)*(z(26)*z(48)+z(31)*z(148)+par.ThighLen*z(23)*z(47)-par.ThighLen*z(24)*z(48)); + z(423) = par.ForearmMass*(z(79)*z(365)+z(88)*z(367)+z(371)*z(94)+z(372)*z(91)) + par.UpparmMass*(z(54)*z(63)+z(57)*z(66)+z(60)*z(364)+z(370)*z(63)); + z(424) = 2*par.ForearmMass*(z(79)*z(371)+z(88)*z(372)) + 2*par.UpparmMass*(z(54)*z(57)+z(60)*z(370)); + z(425) = -par.ForearmMass*(z(79)*z(374)-z(80)*z(371)-z(88)*z(375)-z(89)*z(372)) - par.UpparmMass*(z(55)*z(57)+z(39)*z(43)*z(54)-z(61)*z(370)-z(39)*z(44)*z(60)); + z(426) = z(245)*z(370) + par.ForearmMass*(z(81)*z(371)+z(86)*z(372)); + z(427) = z(73)*z(213)*z(372) + z(74)*z(208)*z(371) + z(75)*z(208)*z(372) + z(76)*z(209)*z(371) + z(77)*z(209)*z(372) + z(79)*z(123)*z(212) + z(79)*z(145)*z(208) + z(79)*z(152)*z(209) + z(109)*z(208)*z(381) + z(109)*z(209)*z(383) + z(109)*z(212)*z(385) + z(121)*z(212)*z(371) + z(123)*z(212)*z(372) + 0.5*Fc*z(79)*z(386) + 0.5*Fc*z(72)*z(79)*z(379) + 0.5*Fc*z(79)*z(113)*z(387) + 0.5*Fc*z(79)*z(117)*z(388) + 0.5*Fc*z(109)*z(115)*z(387) + 0.5*Fc*z(109)*z(119)*z(388) - z(207)*z(371) - z(72)*z(213)*z(371) - z(79)*z(155)*z(210) - z(79)*z(159)*z(211) - z(109)*z(210)*z(389) - z(109)*z(211)*z(390) - z(113)*z(210)*z(371) - z(115)*z(210)*z(372) - z(117)*z(211)*z(371) - z(119)*z(211)*z(372) - 0.5*Fc*z(73)*z(109)*z(379) - 0.5*Fc*z(74)*z(79)*z(380) - 0.5*Fc*z(75)*z(109)*z(380) - 0.5*Fc*z(76)*z(79)*z(382) - 0.5*Fc*z(77)*z(109)*z(382) - 0.5*Fc*z(79)*z(121)*z(384) - 0.5*Fc*z(109)*z(123)*z(384) - z(95)*(z(76)*z(371)+z(77)*z(372)+z(79)*z(152)+z(88)*z(383)) - z(99)*(z(51)*z(57)+z(52)*z(54)+z(52)*z(370)+z(60)*z(151)); + z(428) = par.ForearmMass*(z(79)*z(91)+z(84)*z(94)+z(88)*z(393)+z(396)*z(91)); + z(429) = par.ForearmMass*(z(79)*z(84)+z(88)*z(396)); + z(430) = par.ForearmMass*(z(79)*z(85)+z(80)*z(84)+z(88)*z(398)+z(89)*z(396)); + z(431) = par.ForearmMass*(z(81)*z(84)+z(86)*z(396)+par.UpparmLen*z(72)*z(79)-par.UpparmLen*z(73)*z(88)); + z(432) = z(72)*z(109)*z(213) + z(73)*z(79)*z(213) + z(73)*z(213)*z(396) + z(74)*z(84)*z(208) + z(75)*z(79)*z(208) + z(75)*z(208)*z(396) + z(76)*z(84)*z(209) + z(77)*z(79)*z(209) + z(77)*z(209)*z(396) + z(79)*z(123)*z(212) + z(84)*z(121)*z(212) + z(109)*z(208)*z(404) + z(109)*z(209)*z(406) + z(109)*z(212)*z(385) + z(123)*z(212)*z(396) + 0.5*Fc*z(79)*z(408) + 0.5*Fc*z(72)*z(79)*z(402) + 0.5*Fc*z(79)*z(113)*z(409) + 0.5*Fc*z(79)*z(117)*z(410) + 0.5*Fc*z(109)*z(115)*z(409) + 0.5*Fc*z(109)*z(119)*z(410) - z(84)*z(207) - z(72)*z(84)*z(213) - z(79)*z(115)*z(210) - z(79)*z(119)*z(211) - z(84)*z(113)*z(210) - z(84)*z(117)*z(211) - z(109)*z(210)*z(411) - z(109)*z(211)*z(412) - z(115)*z(210)*z(396) - z(119)*z(211)*z(396) - 0.5*Fc*z(73)*z(109)*z(402) - 0.5*Fc*z(74)*z(79)*z(403) - 0.5*Fc*z(75)*z(109)*z(403) - 0.5*Fc*z(76)*z(79)*z(405) - 0.5*Fc*z(77)*z(109)*z(405) - 0.5*Fc*z(79)*z(121)*z(407) - 0.5*Fc*z(109)*z(123)*z(407) - z(95)*(z(76)*z(84)+z(77)*z(79)+z(77)*z(396)+z(88)*z(406)); + z(433) = par.TrunkInertia*z(224)*z(272) + 2*z(46)*(z(48)+z(138))*z(237); + z(434) = par.TrunkInertia*z(224)*z(269) + 2*z(46)*z(235)*(z(48)+z(138)); + z(435) = 2*z(224)*z(275) + 2*z(46)*z(236)*(z(48)+z(138)); + z(436) = z(98)*z(30)*z(46) + z(74)*z(80)*z(209) + z(75)*z(110)*z(209) + z(80)*z(147)*z(208) + z(110)*z(208)*z(280) + 0.5*Fc*z(80)*z(286) + 0.5*Fc*z(72)*z(80)*z(278) + 0.5*Fc*z(80)*z(113)*z(287) + 0.5*Fc*z(80)*z(117)*z(288) + 0.5*Fc*z(110)*z(115)*z(287) + 0.5*Fc*z(110)*z(119)*z(288) + z(99)*(z(49)*z(55)-z(50)*z(61)) - z(80)*z(154)*z(210) - z(80)*z(158)*z(211) - z(110)*z(210)*z(289) - z(110)*z(211)*z(290) - 0.5*Fc*z(73)*z(110)*z(278) - 0.5*Fc*z(74)*z(80)*z(279) - 0.5*Fc*z(75)*z(110)*z(279) - 0.5*Fc*z(76)*z(80)*z(281) - 0.5*Fc*z(77)*z(110)*z(281) - 0.5*Fc*z(80)*z(121)*z(282) - 0.5*Fc*z(110)*z(123)*z(282) - z(95)*(z(74)*z(80)+z(75)*z(89)); + z(437) = par.TrunkInertia*z(224)*z(272) + 2*z(46)*(z(48)+z(138))*z(237) + par.ForearmMass*(z(80)*z(302)+z(89)*z(304)) - par.TrunkMass*z(30)*z(299) - par.UpparmMass*(z(55)*z(300)-z(61)*z(301)); + z(438) = par.TrunkInertia*z(224)*z(269) + par.TrunkMass*z(30)*z(294) + 2*z(46)*z(235)*(z(48)+z(138)) + par.ForearmMass*(z(80)*z(297)+z(89)*z(303)) - par.UpparmMass*(z(55)*z(295)-z(61)*z(296)); + z(439) = z(98)*z(30)*z(46) + z(74)*z(80)*z(209) + z(75)*z(110)*z(209) + z(80)*z(147)*z(208) + z(110)*z(208)*z(280) + 0.5*Fc*z(80)*z(319) + 0.5*Fc*z(72)*z(80)*z(313) + 0.5*Fc*z(80)*z(113)*z(320) + 0.5*Fc*z(80)*z(117)*z(321) + 0.5*Fc*z(110)*z(115)*z(320) + 0.5*Fc*z(110)*z(119)*z(321) + z(99)*(z(49)*z(55)-z(50)*z(61)) - z(80)*z(157)*z(210) - z(80)*z(158)*z(211) - z(110)*z(210)*z(322) - z(110)*z(211)*z(290) - 0.5*Fc*z(73)*z(110)*z(313) - 0.5*Fc*z(74)*z(80)*z(314) - 0.5*Fc*z(75)*z(110)*z(314) - 0.5*Fc*z(76)*z(80)*z(315) - 0.5*Fc*z(77)*z(110)*z(315) - 0.5*Fc*z(80)*z(121)*z(316) - 0.5*Fc*z(110)*z(123)*z(316) - z(95)*(z(74)*z(80)+z(75)*z(89)); + z(440) = z(346)*z(237) + par.TrunkInertia*z(224)*z(328) + par.ForearmMass*(z(80)*z(336)+z(89)*z(338)) - par.TrunkMass*z(30)*z(333) - par.UpparmMass*(z(55)*z(334)-z(61)*z(335)); + z(441) = z(235)*z(346) + par.TrunkInertia*z(224)*z(325) + par.ForearmMass*(z(80)*z(332)+z(89)*z(337)) - par.TrunkMass*z(30)*z(329) - par.UpparmMass*(z(55)*z(330)-z(61)*z(331)); + z(442) = z(235)*z(346) + par.TrunkInertia*z(224)*z(325) + z(418)*z(24)*z(30) + par.ForearmMass*(z(80)*z(343)+z(89)*z(344)) - par.UpparmMass*(z(55)*z(341)+z(61)*z(342)); + z(443) = z(346)*(z(236)+par.TrunkInertia*z(224)); + z(444) = z(98)*z(30)*z(148) + z(80)*z(146)*z(208) + z(80)*z(153)*z(209) + z(110)*z(208)*z(352) + z(110)*z(209)*z(354) + 0.5*Fc*z(80)*z(356) + 0.5*Fc*z(72)*z(80)*z(350) + 0.5*Fc*z(80)*z(113)*z(357) + 0.5*Fc*z(80)*z(117)*z(358) + 0.5*Fc*z(110)*z(115)*z(357) + 0.5*Fc*z(110)*z(119)*z(358) + z(99)*(z(55)*z(149)-z(61)*z(150)) - z(80)*z(156)*z(210) - z(80)*z(160)*z(211) - z(110)*z(210)*z(359) - z(110)*z(211)*z(360) - 0.5*Fc*z(73)*z(110)*z(350) - 0.5*Fc*z(74)*z(80)*z(351) - 0.5*Fc*z(75)*z(110)*z(351) - 0.5*Fc*z(76)*z(80)*z(353) - 0.5*Fc*z(77)*z(110)*z(353) - 0.5*Fc*z(80)*z(121)*z(355) - 0.5*Fc*z(110)*z(123)*z(355) - z(95)*(z(80)*z(153)+z(89)*z(354)); + z(445) = -par.ForearmMass*(z(374)*z(94)-z(80)*z(365)-z(89)*z(367)-z(375)*z(91)) - par.UpparmMass*(z(55)*z(63)+z(39)*z(43)*z(66)-z(61)*z(364)-z(39)*z(44)*z(63)); + z(446) = 2*par.UpparmMass*z(39)*(z(43)*z(55)+z(44)*z(61)) - 2*par.ForearmMass*(z(80)*z(374)-z(89)*z(375)); + z(447) = z(245)*z(39)*z(44) - par.ForearmMass*(z(81)*z(374)-z(86)*z(375)); + z(448) = z(207)*z(374) + z(72)*z(213)*z(374) + z(73)*z(213)*z(375) + z(75)*z(208)*z(375) + z(77)*z(209)*z(375) + z(80)*z(123)*z(212) + z(80)*z(145)*z(208) + z(80)*z(152)*z(209) + z(110)*z(208)*z(381) + z(110)*z(209)*z(383) + z(110)*z(212)*z(385) + z(113)*z(210)*z(374) + z(117)*z(211)*z(374) + z(123)*z(212)*z(375) + 0.5*Fc*z(80)*z(386) + 0.5*Fc*z(72)*z(80)*z(379) + 0.5*Fc*z(80)*z(113)*z(387) + 0.5*Fc*z(80)*z(117)*z(388) + 0.5*Fc*z(110)*z(115)*z(387) + 0.5*Fc*z(110)*z(119)*z(388) + z(95)*(z(76)*z(374)-z(77)*z(375)-z(80)*z(152)-z(89)*z(383)) + z(99)*(z(52)*z(55)+z(39)*z(43)*z(51)-z(61)*z(151)-z(39)*z(44)*z(52)) - z(74)*z(208)*z(374) - z(76)*z(209)*z(374) - z(80)*z(155)*z(210) - z(80)*z(159)*z(211) - z(110)*z(210)*z(389) - z(110)*z(211)*z(390) - z(115)*z(210)*z(375) - z(119)*z(211)*z(375) - z(121)*z(212)*z(374) - 0.5*Fc*z(73)*z(110)*z(379) - 0.5*Fc*z(74)*z(80)*z(380) - 0.5*Fc*z(75)*z(110)*z(380) - 0.5*Fc*z(76)*z(80)*z(382) - 0.5*Fc*z(77)*z(110)*z(382) - 0.5*Fc*z(80)*z(121)*z(384) - 0.5*Fc*z(110)*z(123)*z(384); + z(449) = par.ForearmMass*(z(80)*z(91)+z(85)*z(94)+z(89)*z(393)+z(398)*z(91)); + z(450) = par.ForearmMass*(z(80)*z(85)+z(89)*z(398)); + z(451) = par.ForearmMass*(z(81)*z(85)+z(86)*z(398)+par.UpparmLen*z(72)*z(80)-par.UpparmLen*z(73)*z(89)); + z(452) = z(72)*z(110)*z(213) + z(73)*z(80)*z(213) + z(73)*z(213)*z(398) + z(74)*z(85)*z(208) + z(75)*z(80)*z(208) + z(75)*z(208)*z(398) + z(76)*z(85)*z(209) + z(77)*z(80)*z(209) + z(77)*z(209)*z(398) + z(80)*z(123)*z(212) + z(85)*z(121)*z(212) + z(110)*z(208)*z(404) + z(110)*z(209)*z(406) + z(110)*z(212)*z(385) + z(123)*z(212)*z(398) + 0.5*Fc*z(80)*z(408) + 0.5*Fc*z(72)*z(80)*z(402) + 0.5*Fc*z(80)*z(113)*z(409) + 0.5*Fc*z(80)*z(117)*z(410) + 0.5*Fc*z(110)*z(115)*z(409) + 0.5*Fc*z(110)*z(119)*z(410) - z(85)*z(207) - z(72)*z(85)*z(213) - z(80)*z(115)*z(210) - z(80)*z(119)*z(211) - z(85)*z(113)*z(210) - z(85)*z(117)*z(211) - z(110)*z(210)*z(411) - z(110)*z(211)*z(412) - z(115)*z(210)*z(398) - z(119)*z(211)*z(398) - 0.5*Fc*z(73)*z(110)*z(402) - 0.5*Fc*z(74)*z(80)*z(403) - 0.5*Fc*z(75)*z(110)*z(403) - 0.5*Fc*z(76)*z(80)*z(405) - 0.5*Fc*z(77)*z(110)*z(405) - 0.5*Fc*z(80)*z(121)*z(407) - 0.5*Fc*z(110)*z(123)*z(407) - z(95)*(z(76)*z(85)+z(77)*z(80)+z(77)*z(398)+z(89)*z(406)); + z(453) = z(74)*z(81)*z(209) + z(75)*z(107)*z(209) + z(81)*z(147)*z(208) + z(107)*z(208)*z(280) + 0.5*Fc*z(81)*z(286) + 0.5*Fc*z(72)*z(81)*z(278) + 0.5*Fc*z(81)*z(113)*z(287) + 0.5*Fc*z(81)*z(117)*z(288) + 0.5*Fc*z(107)*z(115)*z(287) + 0.5*Fc*z(107)*z(119)*z(288) - z(219)*z(50) - z(81)*z(154)*z(210) - z(81)*z(158)*z(211) - z(107)*z(210)*z(289) - z(107)*z(211)*z(290) - 0.5*Fc*z(73)*z(107)*z(278) - 0.5*Fc*z(74)*z(81)*z(279) - 0.5*Fc*z(75)*z(107)*z(279) - 0.5*Fc*z(76)*z(81)*z(281) - 0.5*Fc*z(77)*z(107)*z(281) - 0.5*Fc*z(81)*z(121)*z(282) - 0.5*Fc*z(107)*z(123)*z(282) - z(95)*(z(74)*z(81)+z(75)*z(86)); + z(454) = z(245)*z(301) + par.ForearmMass*(z(81)*z(302)+z(86)*z(304)); + z(455) = z(74)*z(81)*z(209) + z(75)*z(107)*z(209) + z(81)*z(147)*z(208) + z(107)*z(208)*z(280) + 0.5*Fc*z(81)*z(319) + 0.5*Fc*z(72)*z(81)*z(313) + 0.5*Fc*z(81)*z(113)*z(320) + 0.5*Fc*z(81)*z(117)*z(321) + 0.5*Fc*z(107)*z(115)*z(320) + 0.5*Fc*z(107)*z(119)*z(321) - z(219)*z(50) - z(81)*z(157)*z(210) - z(81)*z(158)*z(211) - z(107)*z(210)*z(322) - z(107)*z(211)*z(290) - 0.5*Fc*z(73)*z(107)*z(313) - 0.5*Fc*z(74)*z(81)*z(314) - 0.5*Fc*z(75)*z(107)*z(314) - 0.5*Fc*z(76)*z(81)*z(315) - 0.5*Fc*z(77)*z(107)*z(315) - 0.5*Fc*z(81)*z(121)*z(316) - 0.5*Fc*z(107)*z(123)*z(316) - z(95)*(z(74)*z(81)+z(75)*z(86)); + z(456) = z(245)*z(335) + par.ForearmMass*(z(81)*z(336)+z(86)*z(338)); + z(457) = z(81)*z(146)*z(208) + z(81)*z(153)*z(209) + z(107)*z(208)*z(352) + z(107)*z(209)*z(354) + 0.5*Fc*z(81)*z(356) + 0.5*Fc*z(72)*z(81)*z(350) + 0.5*Fc*z(81)*z(113)*z(357) + 0.5*Fc*z(81)*z(117)*z(358) + 0.5*Fc*z(107)*z(115)*z(357) + 0.5*Fc*z(107)*z(119)*z(358) - z(219)*z(150) - z(81)*z(156)*z(210) - z(81)*z(160)*z(211) - z(107)*z(210)*z(359) - z(107)*z(211)*z(360) - 0.5*Fc*z(73)*z(107)*z(350) - 0.5*Fc*z(74)*z(81)*z(351) - 0.5*Fc*z(75)*z(107)*z(351) - 0.5*Fc*z(76)*z(81)*z(353) - 0.5*Fc*z(77)*z(107)*z(353) - 0.5*Fc*z(81)*z(121)*z(355) - 0.5*Fc*z(107)*z(123)*z(355) - z(95)*(z(81)*z(153)+z(86)*z(354)); + z(458) = z(245)*z(364) + par.ForearmMass*(z(81)*z(365)+z(86)*z(367)); + z(459) = z(81)*z(123)*z(212) + z(81)*z(145)*z(208) + z(81)*z(152)*z(209) + z(107)*z(208)*z(381) + z(107)*z(209)*z(383) + z(107)*z(212)*z(385) + 0.5*Fc*z(81)*z(386) + 0.5*Fc*z(72)*z(81)*z(379) + 0.5*Fc*z(81)*z(113)*z(387) + 0.5*Fc*z(81)*z(117)*z(388) + 0.5*Fc*z(107)*z(115)*z(387) + 0.5*Fc*z(107)*z(119)*z(388) - z(219)*z(151) - z(81)*z(155)*z(210) - z(81)*z(159)*z(211) - z(107)*z(210)*z(389) - z(107)*z(211)*z(390) - 0.5*Fc*z(73)*z(107)*z(379) - 0.5*Fc*z(74)*z(81)*z(380) - 0.5*Fc*z(75)*z(107)*z(380) - 0.5*Fc*z(76)*z(81)*z(382) - 0.5*Fc*z(77)*z(107)*z(382) - 0.5*Fc*z(81)*z(121)*z(384) - 0.5*Fc*z(107)*z(123)*z(384) - z(95)*(z(81)*z(152)+z(86)*z(383)); + z(460) = par.ForearmMass*(z(81)*z(91)+z(86)*z(393)+par.UpparmLen*z(72)*z(94)-par.UpparmLen*z(73)*z(91)); + z(461) = par.ForearmMass*par.UpparmLen; + z(462) = z(461)*(z(72)*z(81)-z(73)*z(86)); + z(463) = z(72)*z(107)*z(213) + z(73)*z(81)*z(213) + z(75)*z(81)*z(208) + z(77)*z(81)*z(209) + z(81)*z(123)*z(212) + z(107)*z(208)*z(404) + z(107)*z(209)*z(406) + z(107)*z(212)*z(385) + 0.5*Fc*z(81)*z(408) + par.UpparmLen*z(72)*z(74)*z(208) + par.UpparmLen*z(72)*z(76)*z(209) + par.UpparmLen*z(72)*z(121)*z(212) + par.UpparmLen*z(73)*z(115)*z(210) + par.UpparmLen*z(73)*z(119)*z(211) + 0.5*Fc*z(72)*z(81)*z(402) + 0.5*Fc*z(81)*z(113)*z(409) + 0.5*Fc*z(81)*z(117)*z(410) + 0.5*Fc*z(107)*z(115)*z(409) + 0.5*Fc*z(107)*z(119)*z(410) - par.UpparmLen*z(72)*z(207) - z(81)*z(115)*z(210) - z(81)*z(119)*z(211) - z(107)*z(210)*z(411) - z(107)*z(211)*z(412) - par.UpparmLen*z(72)*z(113)*z(210) - par.UpparmLen*z(72)*z(117)*z(211) - par.UpparmLen*z(73)*z(75)*z(208) - par.UpparmLen*z(73)*z(77)*z(209) - par.UpparmLen*z(73)*z(123)*z(212) - par.UpparmLen*z(213)*z(72)^2 - par.UpparmLen*z(213)*z(73)^2 - 0.5*Fc*z(73)*z(107)*z(402) - 0.5*Fc*z(74)*z(81)*z(403) - 0.5*Fc*z(75)*z(107)*z(403) - 0.5*Fc*z(76)*z(81)*z(405) - 0.5*Fc*z(77)*z(107)*z(405) - 0.5*Fc*z(81)*z(121)*z(407) - 0.5*Fc*z(107)*z(123)*z(407) - z(95)*(z(77)*z(81)+z(86)*z(406)+par.UpparmLen*z(72)*z(76)-par.UpparmLen*z(73)*z(77)); + z(464) = 0.5*par.ForearmLen*(2*z(75)*z(209)+2*z(208)*z(280)+Fc*z(115)*z(287)+Fc*z(119)*z(288)-2*z(210)*z(289)-2*z(211)*z(290)-Fc*z(73)*z(278)-Fc*z(75)*z(279)-Fc*z(77)*z(281)-Fc*z(123)*z(282)) - z(221)*z(75); + z(465) = 0.5*par.ForearmLen*(2*z(75)*z(209)+2*z(208)*z(280)+Fc*z(115)*z(320)+Fc*z(119)*z(321)-2*z(210)*z(322)-2*z(211)*z(290)-Fc*z(73)*z(313)-Fc*z(75)*z(314)-Fc*z(77)*z(315)-Fc*z(123)*z(316)) - z(221)*z(75); + z(466) = 0.5*par.ForearmLen*(2*z(208)*z(352)+2*z(209)*z(354)+Fc*z(115)*z(357)+Fc*z(119)*z(358)-2*z(210)*z(359)-2*z(211)*z(360)-Fc*z(73)*z(350)-Fc*z(75)*z(351)-Fc*z(77)*z(353)-Fc*z(123)*z(355)) - z(221)*z(354); + z(467) = 0.5*par.ForearmLen*(2*z(208)*z(381)+2*z(209)*z(383)+2*z(212)*z(385)+Fc*z(115)*z(387)+Fc*z(119)*z(388)-2*z(210)*z(389)-2*z(211)*z(390)-Fc*z(73)*z(379)-Fc*z(75)*z(380)-Fc*z(77)*z(382)-Fc*z(123)*z(384)) - z(221)*z(383); + z(468) = 0.5*par.ForearmLen*(2*z(72)*z(213)+2*z(208)*z(404)+2*z(209)*z(406)+2*z(212)*z(385)+Fc*z(115)*z(409)+Fc*z(119)*z(410)-2*z(210)*z(411)-2*z(211)*z(412)-Fc*z(73)*z(402)-Fc*z(75)*z(403)-Fc*z(77)*z(405)-Fc*z(123)*z(407)) - z(221)*z(406); + z(469) = z(273) + z(274)*q1pp + z(274)*q2pp + z(276)*q3pp - z(292); + z(470) = z(305) + z(307)*q1pp + z(309)*q2pp + z(310)*q3pp + z(311)*q4pp + z(247)*z(303)*q5pp - z(324); + z(471) = z(339) + z(340)*q1pp + z(345)*q2pp + z(347)*q3pp + z(348)*q4pp + z(247)*z(337)*q5pp - z(361); + z(472) = z(368) + z(369)*q1pp + z(373)*q2pp + z(376)*q3pp + z(377)*q4pp + z(247)*z(366)*q5pp - z(391); + z(473) = z(394) + z(397)*q2pp + z(399)*q3pp + z(400)*q4pp + 2*z(395)*q1pp + z(247)*z(392)*q5pp - z(413); + z(474) = z(273) + z(274)*q1pp + z(274)*q2pp + z(276)*q3pp - z(414); + z(475) = z(415) + z(274)*q2pp + z(276)*q3pp + z(309)*q1pp - z(416); + z(476) = z(417) + z(345)*q1pp + z(419)*q2pp + z(420)*q3pp + z(421)*q4pp + z(247)*z(344)*q5pp - z(422); + z(477) = z(423) + z(373)*q1pp + z(424)*q2pp + z(425)*q3pp + z(426)*q4pp + z(247)*z(372)*q5pp - z(427); + z(478) = z(428) + z(397)*q1pp + z(430)*q3pp + z(431)*q4pp + 2*z(429)*q2pp + z(247)*z(396)*q5pp - z(432); + z(479) = z(433) + z(434)*q1pp + z(434)*q2pp + z(435)*q3pp - z(436); + z(480) = z(437) + z(434)*q2pp + z(435)*q3pp + z(438)*q1pp - z(439); + z(481) = z(440) + z(441)*q1pp + z(442)*q2pp + z(443)*q3pp - z(444); + z(482) = z(445) + z(376)*q1pp + z(425)*q2pp + z(446)*q3pp + z(447)*q4pp + z(247)*z(375)*q5pp - z(448); + z(483) = z(449) + z(399)*q1pp + z(430)*q2pp + z(451)*q4pp + 2*z(450)*q3pp + z(247)*z(398)*q5pp - z(452); + z(484) = z(454) + z(311)*q1pp - z(455); + z(485) = z(456) + z(348)*q1pp + z(421)*q2pp - z(457); + z(486) = z(458) + z(377)*q1pp + z(426)*q2pp + z(447)*q3pp - z(459); + z(487) = par.UpparmLen*z(247); + z(488) = z(460) + z(400)*q1pp + z(431)*q2pp + z(451)*q3pp + 2*z(462)*q4pp - z(463) - z(487)*z(73)*q5pp; + z(489) = z(247)*z(304) + z(247)*z(303)*q1pp - z(465); + z(490) = z(247)*z(338) + z(247)*z(337)*q1pp + z(247)*z(344)*q2pp - z(466); + z(491) = z(247)*z(367) + z(247)*z(366)*q1pp + z(247)*z(372)*q2pp + z(247)*z(375)*q3pp - z(467); + z(492) = z(247)*z(393) + z(247)*z(392)*q1pp + z(247)*z(396)*q2pp + z(247)*z(398)*q3pp - z(468) - z(487)*z(73)*q4pp; + z(493) = z(45)*z(46)*q1p + z(45)*z(46)*q2p + z(46)*z(268)*q1p + z(46)*z(268)*q2p + z(48)*z(137)*q1p + z(48)*z(137)*q2p + z(137)*z(138)*q1p + z(137)*z(138)*q2p + 2*z(46)*z(48)*q3p + 2*z(46)*z(138)*q3p + z(45)*z(230) + z(46)*z(233) + z(48)*z(228) + z(137)*z(231); z(494) = z(7)*(par.ShankLen*q1p+z(5)); z(495) = -z(21) - z(494) - par.ThighLen*z(18); z(496) = z(8)*(par.ShankLen*q1p+z(5)); @@ -547,7 +550,7 @@ z(506) = -z(17) - z(494) - par.ThighCM*z(18); z(507) = par.TrunkCM*q1p + par.TrunkCM*q2p + z(30)*q3p + z(497) + par.TrunkCM*(z(36)+z(37)); z(508) = z(500) - z(64) - par.UpparmCM*z(65); - z(509) = par.TrunkInertia*z(187)*z(493) + par.ForearmMass*(z(78)*z(504)+z(87)*z(505)) + par.ThighMass*(z(12)*z(506)+z(14)*z(496)) + par.TrunkMass*(z(25)*z(507)+z(32)*z(499)) + par.UpparmMass*(z(53)*z(508)+z(59)*z(502)); + z(509) = par.TrunkInertia*z(223)*z(493) + par.ForearmMass*(z(78)*z(504)+z(87)*z(505)) + par.ThighMass*(z(12)*z(506)+z(14)*z(496)) + par.TrunkMass*(z(25)*z(507)+z(32)*z(499)) + par.UpparmMass*(z(53)*z(508)+z(59)*z(502)); z(510) = par.a*par.Cseat; z(511) = z(510)*z(103)/z(102)^0.5; z(512) = z(9)*z(12)*z(511) + z(10)*z(20)*z(511) - par.Cseat*z(103)*(z(9)*z(20)+z(11)*z(12))/z(102)^0.5; @@ -562,10 +565,10 @@ z(521) = z(72)*z(518) - z(73)*z(517); z(522) = par.TrunkCM*q1p + par.TrunkCM*q2p + z(30)*q3p + par.TrunkCM*(z(36)+z(37)) - z(513); z(523) = z(516) - z(64) - par.UpparmCM*z(65); - z(524) = par.TrunkInertia*z(187)*z(493) + par.ForearmMass*(z(78)*z(520)+z(87)*z(521)) + par.TrunkMass*(z(25)*z(522)+z(32)*z(515)) + par.UpparmMass*(z(53)*z(523)+z(59)*z(518)) - par.ThighMass*z(12)*(z(17)+par.ThighCM*z(18)); + z(524) = par.TrunkInertia*z(223)*z(493) + par.ForearmMass*(z(78)*z(520)+z(87)*z(521)) + par.TrunkMass*(z(25)*z(522)+z(32)*z(515)) + par.UpparmMass*(z(53)*z(523)+z(59)*z(518)) - par.ThighMass*z(12)*(z(17)+par.ThighCM*z(18)); z(525) = z(510)*z(104)/z(102)^0.5; z(526) = z(9)*z(12)*z(525) + z(10)*z(20)*z(525) - par.Cseat*z(104)*(z(9)*z(20)+z(11)*z(12))/z(102)^0.5; - z(527) = z(45)*z(186)*q1p + z(45)*z(186)*q2p + z(46)*z(48)*q1p + z(46)*z(48)*q2p + z(46)*z(184)*q1p + z(46)*z(184)*q2p + z(183)*z(185)*q1p + z(183)*z(185)*q2p + 2*z(46)*z(185)*q3p + 2*z(48)*z(186)*q3p + 2*z(46)*z(195) + 2*z(48)*z(194); + z(527) = z(45)*z(148)*q1p + z(45)*z(148)*q2p + z(46)*z(48)*q1p + z(46)*z(48)*q2p + z(46)*z(138)*q1p + z(46)*z(138)*q2p + z(137)*z(139)*q1p + z(137)*z(139)*q2p + 2*z(46)*z(139)*q3p + 2*z(48)*z(148)*q3p + 2*z(46)*z(231) + 2*z(48)*z(230); z(528) = (1+z(29))*(par.TrunkLen*q1p+par.TrunkLen*q2p+z(39)*q3p) + z(39)*(z(36)+z(37)); z(529) = z(43)*z(528) - z(70) - par.UpparmLen*z(65); z(530) = z(72)*z(529) - z(44)*z(73)*z(528); @@ -573,53 +576,160 @@ z(532) = -z(73)*z(529) - z(44)*z(72)*z(528); z(533) = (1+z(29))*(par.TrunkCM*q1p+par.TrunkCM*q2p+z(30)*q3p) + z(30)*(z(36)+z(37)); z(534) = z(43)*z(528) - z(64) - par.UpparmCM*z(65); - z(535) = par.TrunkInertia*z(187)*z(527) + par.TrunkMass*z(25)*z(533) + par.ForearmMass*(z(78)*z(531)+z(87)*z(532)) + par.UpparmMass*(z(53)*z(534)-z(44)*z(59)*z(528)); + z(535) = par.TrunkInertia*z(223)*z(527) + par.TrunkMass*z(25)*z(533) + par.ForearmMass*(z(78)*z(531)+z(87)*z(532)) + par.UpparmMass*(z(53)*z(534)-z(44)*z(59)*z(528)); z(536) = z(72)*(z(70)+par.UpparmLen*z(65)); z(537) = -z(92) - z(536) - par.ForearmCM*z(93); z(538) = z(73)*(z(70)+par.UpparmLen*z(65)); z(539) = par.ForearmMass*(z(78)*z(537)+z(87)*z(538)) - par.UpparmMass*z(53)*(z(64)+par.UpparmCM*z(65)); z(540) = par.ForearmMass*z(78)*(z(92)+par.ForearmCM*z(93)); - z(541) = z(205)*z(496) + par.TrunkInertia*z(187)*z(493) + par.ForearmMass*(z(79)*z(504)+z(88)*z(505)) + par.TrunkMass*(z(26)*z(507)+z(31)*z(499)) + par.UpparmMass*(z(54)*z(508)+z(60)*z(502)); + z(541) = z(241)*z(496) + par.TrunkInertia*z(223)*z(493) + par.ForearmMass*(z(79)*z(504)+z(88)*z(505)) + par.TrunkMass*(z(26)*z(507)+z(31)*z(499)) + par.UpparmMass*(z(54)*z(508)+z(60)*z(502)); z(542) = par.ThighLen*(z(10)*z(511)-par.Cseat*z(9)*z(103)/z(102)^0.5); - z(543) = par.TrunkInertia*z(187)*z(493) + par.ForearmMass*(z(79)*z(520)+z(88)*z(521)) + par.TrunkMass*(z(26)*z(522)+z(31)*z(515)) + par.UpparmMass*(z(54)*z(523)+z(60)*z(518)); + z(543) = par.TrunkInertia*z(223)*z(493) + par.ForearmMass*(z(79)*z(520)+z(88)*z(521)) + par.TrunkMass*(z(26)*z(522)+z(31)*z(515)) + par.UpparmMass*(z(54)*z(523)+z(60)*z(518)); z(544) = par.ThighLen*(z(10)*z(525)-par.Cseat*z(9)*z(104)/z(102)^0.5); - z(545) = par.TrunkInertia*z(187)*z(527) + par.TrunkMass*z(26)*z(533) + par.ForearmMass*(z(79)*z(531)+z(88)*z(532)) + par.UpparmMass*(z(54)*z(534)-z(44)*z(60)*z(528)); + z(545) = par.TrunkInertia*z(223)*z(527) + par.TrunkMass*z(26)*z(533) + par.ForearmMass*(z(79)*z(531)+z(88)*z(532)) + par.UpparmMass*(z(54)*z(534)-z(44)*z(60)*z(528)); z(546) = par.ForearmMass*(z(79)*z(537)+z(88)*z(538)) - par.UpparmMass*z(54)*(z(64)+par.UpparmCM*z(65)); z(547) = par.ForearmMass*z(79)*(z(92)+par.ForearmCM*z(93)); - z(548) = par.TrunkInertia*z(188)*z(493) + par.ForearmMass*(z(80)*z(504)+z(89)*z(505)) - par.TrunkMass*z(30)*z(499) - par.UpparmMass*(z(55)*z(508)-z(61)*z(502)); - z(549) = par.TrunkInertia*z(188)*z(493) + par.ForearmMass*(z(80)*z(520)+z(89)*z(521)) - par.TrunkMass*z(30)*z(515) - par.UpparmMass*(z(55)*z(523)-z(61)*z(518)); - z(550) = par.TrunkInertia*z(188)*z(527) + par.ForearmMass*(z(80)*z(531)+z(89)*z(532)) - par.UpparmMass*(z(55)*z(534)+z(44)*z(61)*z(528)); + z(548) = par.TrunkInertia*z(224)*z(493) + par.ForearmMass*(z(80)*z(504)+z(89)*z(505)) - par.TrunkMass*z(30)*z(499) - par.UpparmMass*(z(55)*z(508)-z(61)*z(502)); + z(549) = par.TrunkInertia*z(224)*z(493) + par.ForearmMass*(z(80)*z(520)+z(89)*z(521)) - par.TrunkMass*z(30)*z(515) - par.UpparmMass*(z(55)*z(523)-z(61)*z(518)); + z(550) = par.TrunkInertia*z(224)*z(527) + par.ForearmMass*(z(80)*z(531)+z(89)*z(532)) - par.UpparmMass*(z(55)*z(534)+z(44)*z(61)*z(528)); z(551) = par.ForearmMass*(z(80)*z(537)+z(89)*z(538)) + par.UpparmMass*z(55)*(z(64)+par.UpparmCM*z(65)); z(552) = par.ForearmMass*z(80)*(z(92)+par.ForearmCM*z(93)); - z(553) = z(209)*z(502) + par.ForearmMass*(z(81)*z(504)+z(86)*z(505)); - z(554) = z(209)*z(518) + par.ForearmMass*(z(81)*z(520)+z(86)*z(521)); - z(555) = par.ForearmMass*(z(81)*z(531)+z(86)*z(532)) - z(209)*z(44)*z(528); + z(553) = z(245)*z(502) + par.ForearmMass*(z(81)*z(504)+z(86)*z(505)); + z(554) = z(245)*z(518) + par.ForearmMass*(z(81)*z(520)+z(86)*z(521)); + z(555) = par.ForearmMass*(z(81)*z(531)+z(86)*z(532)) - z(245)*z(44)*z(528); z(556) = par.ForearmMass*(z(81)*z(537)+z(86)*z(538)); z(557) = par.ForearmMass*z(81)*(z(92)+par.ForearmCM*z(93)); z(558) = z(509) - z(512); z(559) = z(524) - z(526); z(560) = z(541) - z(542); z(561) = z(543) - z(544); - z(562) = z(211)*z(505); - z(563) = z(211)*z(521); - z(564) = z(211)*z(532); - z(565) = z(211)*z(538); - z(566) = z(73)*z(108)*z(165) + z(74)*z(78)*z(160) + z(75)*z(108)*z(160) + z(76)*z(78)*z(161) + z(77)*z(108)*z(161) + z(78)*z(121)*z(164) + z(108)*z(123)*z(164) - z(78)*z(159) - z(72)*z(78)*z(165) - z(78)*z(113)*z(162) - z(78)*z(117)*z(163) - z(108)*z(115)*z(162) - z(108)*z(119)*z(163); - z(567) = z(73)*z(109)*z(165) + z(74)*z(79)*z(160) + z(75)*z(109)*z(160) + z(76)*z(79)*z(161) + z(77)*z(109)*z(161) + z(79)*z(121)*z(164) + z(109)*z(123)*z(164) - z(79)*z(159) - z(72)*z(79)*z(165) - z(79)*z(113)*z(162) - z(79)*z(117)*z(163) - z(109)*z(115)*z(162) - z(109)*z(119)*z(163); - z(568) = z(73)*z(110)*z(165) + z(74)*z(80)*z(160) + z(75)*z(110)*z(160) + z(76)*z(80)*z(161) + z(77)*z(110)*z(161) + z(80)*z(121)*z(164) + z(110)*z(123)*z(164) - z(80)*z(159) - z(72)*z(80)*z(165) - z(80)*z(113)*z(162) - z(80)*z(117)*z(163) - z(110)*z(115)*z(162) - z(110)*z(119)*z(163); - z(569) = z(73)*z(107)*z(165) + z(74)*z(81)*z(160) + z(75)*z(107)*z(160) + z(76)*z(81)*z(161) + z(77)*z(107)*z(161) + z(81)*z(121)*z(164) + z(107)*z(123)*z(164) - z(81)*z(159) - z(72)*z(81)*z(165) - z(81)*z(113)*z(162) - z(81)*z(117)*z(163) - z(107)*z(115)*z(162) - z(107)*z(119)*z(163); - z(570) = par.ForearmLen*(z(115)*z(162)+z(119)*z(163)-z(73)*z(165)-z(75)*z(160)-z(77)*z(161)-z(123)*z(164)); + z(562) = z(247)*z(505); + z(563) = z(247)*z(521); + z(564) = z(247)*z(532); + z(565) = z(247)*z(538); + z(566) = z(73)*z(108)*z(206) + z(74)*z(78)*z(201) + z(75)*z(108)*z(201) + z(76)*z(78)*z(202) + z(77)*z(108)*z(202) + z(78)*z(121)*z(205) + z(108)*z(123)*z(205) - z(78)*z(200) - z(72)*z(78)*z(206) - z(78)*z(113)*z(203) - z(78)*z(117)*z(204) - z(108)*z(115)*z(203) - z(108)*z(119)*z(204); + z(567) = z(73)*z(109)*z(206) + z(74)*z(79)*z(201) + z(75)*z(109)*z(201) + z(76)*z(79)*z(202) + z(77)*z(109)*z(202) + z(79)*z(121)*z(205) + z(109)*z(123)*z(205) - z(79)*z(200) - z(72)*z(79)*z(206) - z(79)*z(113)*z(203) - z(79)*z(117)*z(204) - z(109)*z(115)*z(203) - z(109)*z(119)*z(204); + z(568) = z(73)*z(110)*z(206) + z(74)*z(80)*z(201) + z(75)*z(110)*z(201) + z(76)*z(80)*z(202) + z(77)*z(110)*z(202) + z(80)*z(121)*z(205) + z(110)*z(123)*z(205) - z(80)*z(200) - z(72)*z(80)*z(206) - z(80)*z(113)*z(203) - z(80)*z(117)*z(204) - z(110)*z(115)*z(203) - z(110)*z(119)*z(204); + z(569) = z(73)*z(107)*z(206) + z(74)*z(81)*z(201) + z(75)*z(107)*z(201) + z(76)*z(81)*z(202) + z(77)*z(107)*z(202) + z(81)*z(121)*z(205) + z(107)*z(123)*z(205) - z(81)*z(200) - z(72)*z(81)*z(206) - z(81)*z(113)*z(203) - z(81)*z(117)*z(204) - z(107)*z(115)*z(203) - z(107)*z(119)*z(204); + z(570) = par.ForearmLen*(z(115)*z(203)+z(119)*z(204)-z(73)*z(206)-z(75)*z(201)-z(77)*z(202)-z(123)*z(205)); + z(571) = z(11)*z(23) - z(24)*z(136); + z(572) = z(43)*z(268) + z(44)*z(571); + z(573) = z(43)*z(571) - z(44)*z(268); + z(574) = z(72)*z(572) + z(73)*z(573); + z(575) = z(1)*(z(74)+z(574)) - z(2)*(z(76)+z(147)); + z(576) = z(9)*z(574) + z(10)*z(76) + z(11)*z(147) + z(74)*z(136) + 2*z(9)*z(74) + 2*z(10)*z(147); + z(577) = z(1)*(z(49)+z(572)) - z(2)*(z(51)+z(141)); + z(578) = z(1)*(z(51)+z(141)) + z(2)*(z(49)+z(572)); + z(579) = z(7)*z(577) - z(8)*z(578); + z(580) = z(1)*(z(45)+z(268)) - z(2)*(z(47)+z(137)); + z(581) = ((z(172)*z(154)+z(173)*z(158)+z(174)*z(161)+z(175)*z(2)+z(176)*z(168)+z(177)*z(137)+z(178)*z(45)-z(179)*z(147)-z(180)*z(74)-z(181)*z(170)-z(182)*z(1)-z(183)*z(10)-z(184)*z(9)-z(185)*z(141)-z(186)*z(49))^2-(z(172)*z(575)+z(173)*z(576)+z(174)*z(577)+z(175)*z(1)+z(176)*z(579)+z(177)*z(268)+z(178)*z(137)+z(182)*z(2)-z(179)*z(574)-z(180)*z(147)-z(181)*z(580)-z(183)*z(136)-z(184)*z(10)-z(185)*z(572)-z(186)*z(141))*(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43)))/(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43))^1.5; + z(582) = z(1)*(z(74)+z(574)); + z(583) = z(1)*(z(49)+z(572)); + z(584) = z(2)*(z(49)+z(572)); + z(585) = z(7)*z(583) - z(7)*z(164) - z(8)*z(161) - z(8)*z(584); + z(586) = z(1)*(z(45)+z(268)); + z(587) = ((z(172)*z(157)+z(173)*z(158)+z(174)*z(163)+z(176)*z(167)+z(177)*z(137)+z(178)*z(45)-z(179)*z(147)-z(180)*z(74)-z(181)*z(171)-z(183)*z(10)-z(184)*z(9)-z(185)*z(141)-z(186)*z(49)-z(189)*z(8))*(z(172)*z(154)+z(173)*z(158)+z(174)*z(161)+z(175)*z(2)+z(176)*z(168)+z(177)*z(137)+z(178)*z(45)-z(179)*z(147)-z(180)*z(74)-z(181)*z(170)-z(182)*z(1)-z(183)*z(10)-z(184)*z(9)-z(185)*z(141)-z(186)*z(49))-(z(172)*z(582)+z(173)*z(576)+z(174)*z(583)+z(176)*z(585)+z(177)*z(268)+z(178)*z(137)-z(179)*z(574)-z(180)*z(147)-z(181)*z(586)-z(183)*z(136)-z(184)*z(10)-z(185)*z(572)-z(186)*z(141))*(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43)))/(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43))^1.5; + z(588) = par.ForearmLen*((par.ShankLen*z(115)+par.ThighLen*z(119)-par.TrunkLen*z(123)-par.UpparmLen*z(73)-z(111)*z(75)-z(112)*z(77))*(z(172)*z(154)+z(173)*z(158)+z(174)*z(161)+z(175)*z(2)+z(176)*z(168)+z(177)*z(137)+z(178)*z(45)-z(179)*z(147)-z(180)*z(74)-z(181)*z(170)-z(182)*z(1)-z(183)*z(10)-z(184)*z(9)-z(185)*z(141)-z(186)*z(49))-(par.ShankLen*z(289)+par.ThighLen*z(290)-z(111)*z(280)-z(112)*z(75))*(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43)))/(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43))^1.5; + z(589) = -z(10)*z(23) - z(24)*z(136); + z(590) = z(43)*z(138) + z(44)*z(589); + z(591) = z(43)*z(589) - z(44)*z(138); + z(592) = z(72)*z(590) + z(73)*z(591); + z(593) = z(1)*(z(48)+z(138)); + z(594) = z(1)*(z(153)+z(592)); + z(595) = z(9)*z(153) + z(9)*z(592) + z(10)*z(146) + z(11)*z(146); + z(596) = z(1)*(z(149)+z(590)); + z(597) = z(2)*(z(149)+z(590)); + z(598) = z(7)*z(596) - z(8)*z(597); + z(599) = ((z(179)*z(146)+z(180)*z(153)+z(181)*z(125)+z(185)*z(140)+z(186)*z(149)-z(172)*z(156)-z(173)*z(160)-z(174)*z(162)-z(176)*z(169)-z(177)*z(46)-z(178)*z(48)-z(191)*z(24))*(z(172)*z(154)+z(173)*z(158)+z(174)*z(161)+z(175)*z(2)+z(176)*z(168)+z(177)*z(137)+z(178)*z(45)-z(179)*z(147)-z(180)*z(74)-z(181)*z(170)-z(182)*z(1)-z(183)*z(10)-z(184)*z(9)-z(185)*z(141)-z(186)*z(49))+(z(172)*z(594)+z(173)*z(595)+z(174)*z(596)+z(176)*z(598)+z(177)*z(138)+z(178)*z(46)-z(179)*z(592)-z(180)*z(146)-z(181)*z(593)-z(185)*z(590)-z(186)*z(140))*(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43)))/(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43))^1.5; + z(600) = -z(43)*z(137) - z(44)*z(138); + z(601) = z(72)*z(142) + z(73)*z(600); + z(602) = z(1)*(z(152)+z(601)); + z(603) = z(9)*z(152) + z(9)*z(601) + z(10)*z(145) + z(11)*z(145); + z(604) = z(1)*(z(52)+z(142)); + z(605) = z(2)*(z(52)+z(142)); + z(606) = z(7)*z(604) - z(8)*z(605); + z(607) = ((z(179)*z(145)+z(180)*z(152)+z(185)*z(50)+z(186)*z(52)+z(190)*z(123)-z(172)*z(155)-z(173)*z(159)-z(174)*z(129)-z(176)*z(133)-z(192)*z(44))*(z(172)*z(154)+z(173)*z(158)+z(174)*z(161)+z(175)*z(2)+z(176)*z(168)+z(177)*z(137)+z(178)*z(45)-z(179)*z(147)-z(180)*z(74)-z(181)*z(170)-z(182)*z(1)-z(183)*z(10)-z(184)*z(9)-z(185)*z(141)-z(186)*z(49))+(z(172)*z(602)+z(173)*z(603)+z(174)*z(604)+z(176)*z(606)-z(179)*z(601)-z(180)*z(145)-z(185)*z(142)-z(186)*z(50))*(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43)))/(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43))^1.5; + z(608) = z(599)*q3p + z(607)*q4p - z(581)*q1p - z(587)*q2p - z(588)*q5p; + z(609) = z(1)*z(7)*(z(49)+z(572)) - z(7)*z(164) - z(8)*z(161) - z(2)*z(8)*(z(49)+z(572)); + z(610) = ((z(172)*z(157)+z(173)*z(158)+z(174)*z(163)+z(176)*z(167)+z(177)*z(137)+z(178)*z(45)-z(179)*z(147)-z(180)*z(74)-z(181)*z(171)-z(183)*z(10)-z(184)*z(9)-z(185)*z(141)-z(186)*z(49)-z(189)*z(8))*(z(172)*z(154)+z(173)*z(158)+z(174)*z(161)+z(175)*z(2)+z(176)*z(168)+z(177)*z(137)+z(178)*z(45)-z(179)*z(147)-z(180)*z(74)-z(181)*z(170)-z(182)*z(1)-z(183)*z(10)-z(184)*z(9)-z(185)*z(141)-z(186)*z(49))-(z(173)*z(576)+z(176)*z(609)+z(177)*z(268)+z(178)*z(137)+z(172)*z(1)*(z(74)+z(574))+z(174)*z(1)*(z(49)+z(572))-z(179)*z(574)-z(180)*z(147)-z(183)*z(136)-z(184)*z(10)-z(185)*z(572)-z(186)*z(141)-z(181)*z(1)*(z(45)+z(268)))*(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43)))/(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43))^1.5; + z(611) = z(1)*z(574) + z(2)*z(147); + z(612) = z(1)*z(572) + z(2)*z(141); + z(613) = z(1)*z(141) - z(2)*z(572); + z(614) = z(7)*z(612) + z(8)*z(613) + 2*z(7)*z(165) - 2*z(8)*z(163) - z(7)*z(128) - z(8)*z(130); + z(615) = z(1)*z(268) + z(2)*z(137); + z(616) = ((z(172)*z(157)+z(173)*z(158)+z(174)*z(163)+z(176)*z(167)+z(177)*z(137)+z(178)*z(45)-z(179)*z(147)-z(180)*z(74)-z(181)*z(171)-z(183)*z(10)-z(184)*z(9)-z(185)*z(141)-z(186)*z(49)-z(189)*z(8))^2-(z(172)*z(611)+z(173)*z(576)+z(174)*z(612)+z(176)*z(614)+z(177)*z(268)+z(178)*z(137)-z(179)*z(574)-z(180)*z(147)-z(181)*z(615)-z(183)*z(136)-z(184)*z(10)-z(185)*z(572)-z(186)*z(141)-z(189)*z(7))*(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43)))/(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43))^1.5; + z(617) = par.ForearmLen*((par.ShankLen*z(115)+par.ThighLen*z(119)-par.TrunkLen*z(123)-par.UpparmLen*z(73)-z(111)*z(75)-z(112)*z(77))*(z(172)*z(157)+z(173)*z(158)+z(174)*z(163)+z(176)*z(167)+z(177)*z(137)+z(178)*z(45)-z(179)*z(147)-z(180)*z(74)-z(181)*z(171)-z(183)*z(10)-z(184)*z(9)-z(185)*z(141)-z(186)*z(49)-z(189)*z(8))-(par.ShankLen*z(322)+par.ThighLen*z(290)-z(111)*z(280)-z(112)*z(75))*(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43)))/(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43))^1.5; + z(618) = z(1)*z(138) + z(2)*z(46); + z(619) = z(1)*z(592) + z(2)*z(146); + z(620) = z(1)*z(590) + z(2)*z(140); + z(621) = z(1)*z(140) - z(2)*z(590); + z(622) = z(7)*z(166) + z(7)*z(620) + z(8)*z(621) - z(8)*z(162); + z(623) = ((z(179)*z(146)+z(180)*z(153)+z(181)*z(125)+z(185)*z(140)+z(186)*z(149)-z(172)*z(156)-z(173)*z(160)-z(174)*z(162)-z(176)*z(169)-z(177)*z(46)-z(178)*z(48)-z(191)*z(24))*(z(172)*z(157)+z(173)*z(158)+z(174)*z(163)+z(176)*z(167)+z(177)*z(137)+z(178)*z(45)-z(179)*z(147)-z(180)*z(74)-z(181)*z(171)-z(183)*z(10)-z(184)*z(9)-z(185)*z(141)-z(186)*z(49)-z(189)*z(8))+(z(172)*z(619)+z(173)*z(595)+z(174)*z(620)+z(176)*z(622)+z(177)*z(138)+z(178)*z(46)-z(179)*z(592)-z(180)*z(146)-z(181)*z(618)-z(185)*z(590)-z(186)*z(140))*(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43)))/(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43))^1.5; + z(624) = z(1)*z(601) + z(2)*z(145); + z(625) = z(1)*z(142) + z(2)*z(50); + z(626) = z(1)*z(50) - z(2)*z(142); + z(627) = z(7)*z(131) + z(7)*z(625) + z(8)*z(626) - z(8)*z(129); + z(628) = ((z(179)*z(145)+z(180)*z(152)+z(185)*z(50)+z(186)*z(52)+z(190)*z(123)-z(172)*z(155)-z(173)*z(159)-z(174)*z(129)-z(176)*z(133)-z(192)*z(44))*(z(172)*z(157)+z(173)*z(158)+z(174)*z(163)+z(176)*z(167)+z(177)*z(137)+z(178)*z(45)-z(179)*z(147)-z(180)*z(74)-z(181)*z(171)-z(183)*z(10)-z(184)*z(9)-z(185)*z(141)-z(186)*z(49)-z(189)*z(8))+(z(172)*z(624)+z(173)*z(603)+z(174)*z(625)+z(176)*z(627)-z(179)*z(601)-z(180)*z(145)-z(185)*z(142)-z(186)*z(50))*(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43)))/(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43))^1.5; + z(629) = z(623)*q3p + z(628)*q4p - z(610)*q1p - z(616)*q2p - z(617)*q5p; + z(630) = (z(149)+z(590))*(z(1)*z(7)-z(2)*z(8)); + z(631) = ((z(179)*z(146)+z(180)*z(153)+z(181)*z(125)+z(185)*z(140)+z(186)*z(149)-z(172)*z(156)-z(173)*z(160)-z(174)*z(162)-z(176)*z(169)-z(177)*z(46)-z(178)*z(48)-z(191)*z(24))*(z(172)*z(154)+z(173)*z(158)+z(174)*z(161)+z(175)*z(2)+z(176)*z(168)+z(177)*z(137)+z(178)*z(45)-z(179)*z(147)-z(180)*z(74)-z(181)*z(170)-z(182)*z(1)-z(183)*z(10)-z(184)*z(9)-z(185)*z(141)-z(186)*z(49))+(z(173)*z(595)+z(176)*z(630)+z(177)*z(138)+z(178)*z(46)+z(172)*z(1)*(z(153)+z(592))+z(174)*z(1)*(z(149)+z(590))-z(179)*z(592)-z(180)*z(146)-z(185)*z(590)-z(186)*z(140)-z(181)*z(1)*(z(48)+z(138)))*(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43)))/(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43))^1.5; + z(632) = par.ForearmLen*((par.ShankLen*z(115)+par.ThighLen*z(119)-par.TrunkLen*z(123)-par.UpparmLen*z(73)-z(111)*z(75)-z(112)*z(77))*(z(179)*z(146)+z(180)*z(153)+z(181)*z(125)+z(185)*z(140)+z(186)*z(149)-z(172)*z(156)-z(173)*z(160)-z(174)*z(162)-z(176)*z(169)-z(177)*z(46)-z(178)*z(48)-z(191)*z(24))+(par.ShankLen*z(359)+par.ThighLen*z(360)-z(111)*z(352)-z(112)*z(354))*(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43)))/(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43))^1.5; + z(633) = z(9)*z(24) - z(10)*z(23); + z(634) = z(43)*z(139) + z(44)*z(633); + z(635) = z(43)*z(633) - z(44)*z(139); + z(636) = z(72)*z(634) + z(73)*z(635); + z(637) = z(11)*z(24) - z(9)*z(23); + z(638) = z(43)*z(148) + z(44)*z(637); + z(639) = z(43)*z(637) - z(44)*z(148); + z(640) = z(72)*z(638) + z(73)*z(639); + z(641) = z(1)*z(139) + z(2)*z(148); + z(642) = z(1)*z(636) + z(2)*z(640); + z(643) = z(9)*z(636) + z(11)*z(640); + z(644) = z(1)*z(634) + z(2)*z(638); + z(645) = z(1)*z(638) - z(2)*z(634); + z(646) = z(7)*z(644) + z(8)*z(645); + z(647) = ((z(179)*z(146)+z(180)*z(153)+z(181)*z(125)+z(185)*z(140)+z(186)*z(149)-z(172)*z(156)-z(173)*z(160)-z(174)*z(162)-z(176)*z(169)-z(177)*z(46)-z(178)*z(48)-z(191)*z(24))^2+(z(179)*z(636)+z(180)*z(640)+z(181)*z(641)+z(185)*z(634)+z(186)*z(638)-z(172)*z(642)-z(173)*z(643)-z(174)*z(644)-z(176)*z(646)-z(177)*z(139)-z(178)*z(148)-z(191)*z(23))*(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43)))/(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43))^1.5; + z(648) = -z(43)*z(46) - z(44)*z(139); + z(649) = z(72)*z(143) + z(73)*z(648); + z(650) = -z(43)*z(48) - z(44)*z(148); + z(651) = z(72)*z(150) + z(73)*z(650); + z(652) = z(1)*z(649) + z(2)*z(651); + z(653) = z(9)*z(649) + z(11)*z(651); + z(654) = z(1)*z(143) + z(2)*z(150); + z(655) = z(1)*z(150) - z(2)*z(143); + z(656) = z(7)*z(654) + z(8)*z(655); + z(657) = ((z(179)*z(145)+z(180)*z(152)+z(185)*z(50)+z(186)*z(52)+z(190)*z(123)-z(172)*z(155)-z(173)*z(159)-z(174)*z(129)-z(176)*z(133)-z(192)*z(44))*(z(179)*z(146)+z(180)*z(153)+z(181)*z(125)+z(185)*z(140)+z(186)*z(149)-z(172)*z(156)-z(173)*z(160)-z(174)*z(162)-z(176)*z(169)-z(177)*z(46)-z(178)*z(48)-z(191)*z(24))-(z(172)*z(652)+z(173)*z(653)+z(174)*z(654)+z(176)*z(656)-z(179)*z(649)-z(180)*z(651)-z(185)*z(143)-z(186)*z(150))*(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43)))/(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43))^1.5; + z(658) = z(623)*q2p + z(631)*q1p + z(632)*q5p - z(647)*q3p - z(657)*q4p; + z(659) = (z(52)+z(142))*(z(1)*z(7)-z(2)*z(8)); + z(660) = ((z(179)*z(145)+z(180)*z(152)+z(185)*z(50)+z(186)*z(52)+z(190)*z(123)-z(172)*z(155)-z(173)*z(159)-z(174)*z(129)-z(176)*z(133)-z(192)*z(44))*(z(172)*z(154)+z(173)*z(158)+z(174)*z(161)+z(175)*z(2)+z(176)*z(168)+z(177)*z(137)+z(178)*z(45)-z(179)*z(147)-z(180)*z(74)-z(181)*z(170)-z(182)*z(1)-z(183)*z(10)-z(184)*z(9)-z(185)*z(141)-z(186)*z(49))-(z(179)*z(601)+z(180)*z(145)+z(185)*z(142)+z(186)*z(50)-z(173)*z(603)-z(176)*z(659)-z(172)*z(1)*(z(152)+z(601))-z(174)*z(1)*(z(52)+z(142)))*(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43)))/(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43))^1.5; + z(661) = par.ForearmLen*((par.ShankLen*z(115)+par.ThighLen*z(119)-par.TrunkLen*z(123)-par.UpparmLen*z(73)-z(111)*z(75)-z(112)*z(77))*(z(179)*z(145)+z(180)*z(152)+z(185)*z(50)+z(186)*z(52)+z(190)*z(123)-z(172)*z(155)-z(173)*z(159)-z(174)*z(129)-z(176)*z(133)-z(192)*z(44))+(par.ShankLen*z(389)+par.ThighLen*z(390)-par.TrunkLen*z(385)-z(111)*z(381)-z(112)*z(383))*(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43)))/(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43))^1.5; + z(662) = z(44)*z(45) - z(43)*z(46); + z(663) = z(72)*z(144) + z(73)*z(662); + z(664) = z(44)*z(47) - z(43)*z(48); + z(665) = z(72)*z(151) + z(73)*z(664); + z(666) = z(1)*z(663) + z(2)*z(665); + z(667) = z(9)*z(663) + z(11)*z(665); + z(668) = z(1)*z(144) + z(2)*z(151); + z(669) = z(1)*z(151) - z(2)*z(144); + z(670) = z(7)*z(668) + z(8)*z(669); + z(671) = ((z(179)*z(145)+z(180)*z(152)+z(185)*z(50)+z(186)*z(52)+z(190)*z(123)-z(172)*z(155)-z(173)*z(159)-z(174)*z(129)-z(176)*z(133)-z(192)*z(44))^2+(z(179)*z(663)+z(180)*z(665)+z(185)*z(144)+z(186)*z(151)+z(190)*z(385)-z(172)*z(666)-z(173)*z(667)-z(174)*z(668)-z(176)*z(670)-z(192)*z(43))*(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43)))/(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43))^1.5; + z(672) = z(628)*q2p + z(660)*q1p + z(661)*q5p - z(657)*q3p - z(671)*q4p; + z(673) = ((z(172)*z(115)+z(173)*z(119)-z(179)*z(75)-z(180)*z(77)-z(188)*z(73)-z(190)*z(123))*(z(172)*z(154)+z(173)*z(158)+z(174)*z(161)+z(175)*z(2)+z(176)*z(168)+z(177)*z(137)+z(178)*z(45)-z(179)*z(147)-z(180)*z(74)-z(181)*z(170)-z(182)*z(1)-z(183)*z(10)-z(184)*z(9)-z(185)*z(141)-z(186)*z(49))+(z(179)*z(280)+z(180)*z(75)-z(173)*z(290)-z(172)*z(1)*(z(77)+z(280)))*(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43)))/(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43))^1.5; + z(674) = ((z(172)*z(115)+z(173)*z(119)-z(179)*z(75)-z(180)*z(77)-z(188)*z(73)-z(190)*z(123))*(z(172)*z(157)+z(173)*z(158)+z(174)*z(163)+z(176)*z(167)+z(177)*z(137)+z(178)*z(45)-z(179)*z(147)-z(180)*z(74)-z(181)*z(171)-z(183)*z(10)-z(184)*z(9)-z(185)*z(141)-z(186)*z(49)-z(189)*z(8))-(z(172)*z(322)+z(173)*z(290)-z(179)*z(280)-z(180)*z(75))*(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43)))/(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43))^1.5; + z(675) = par.ForearmLen*((par.ShankLen*z(115)+par.ThighLen*z(119)-par.TrunkLen*z(123)-par.UpparmLen*z(73)-z(111)*z(75)-z(112)*z(77))*(z(172)*z(115)+z(173)*z(119)-z(179)*z(75)-z(180)*z(77)-z(188)*z(73)-z(190)*z(123))-(par.ShankLen*z(411)+par.ThighLen*z(412)-par.TrunkLen*z(385)-par.UpparmLen*z(72)-z(111)*z(404)-z(112)*z(406))*(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43)))/(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43))^1.5; + z(676) = ((z(172)*z(115)+z(173)*z(119)-z(179)*z(75)-z(180)*z(77)-z(188)*z(73)-z(190)*z(123))*(z(179)*z(146)+z(180)*z(153)+z(181)*z(125)+z(185)*z(140)+z(186)*z(149)-z(172)*z(156)-z(173)*z(160)-z(174)*z(162)-z(176)*z(169)-z(177)*z(46)-z(178)*z(48)-z(191)*z(24))+(z(172)*z(359)+z(173)*z(360)-z(179)*z(352)-z(180)*z(354))*(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43)))/(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43))^1.5; + z(677) = ((z(172)*z(115)+z(173)*z(119)-z(179)*z(75)-z(180)*z(77)-z(188)*z(73)-z(190)*z(123))*(z(179)*z(145)+z(180)*z(152)+z(185)*z(50)+z(186)*z(52)+z(190)*z(123)-z(172)*z(155)-z(173)*z(159)-z(174)*z(129)-z(176)*z(133)-z(192)*z(44))+(z(172)*z(389)+z(173)*z(390)-z(179)*z(381)-z(180)*z(383)-z(190)*z(385))*(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43)))/(z(187)+2*z(172)*z(113)+2*z(173)*z(117)+2*z(174)*z(128)+2*z(176)*z(132)+2*z(177)*z(45)+2*z(178)*z(47)+2*z(188)*z(72)+2*z(189)*z(7)-2*z(175)*z(1)-2*z(179)*z(74)-2*z(180)*z(76)-2*z(181)*z(124)-2*z(182)*z(2)-2*z(183)*z(9)-2*z(184)*z(11)-2*z(185)*z(49)-2*z(186)*z(51)-2*z(190)*z(121)-2*z(191)*z(23)-2*z(192)*z(43))^1.5; + z(678) = z(676)*q3p + z(677)*q4p - z(673)*q1p - z(674)*q2p - z(675)*q5p; %=========================================================================== - f(1) = z(213) + z(203)*q1pp + z(206)*q2pp + z(207)*q3pp + z(210)*q4pp + z(212)*q5pp - z(174); - f(2) = z(219) + z(206)*q1pp + z(215)*q2pp + z(216)*q3pp + z(217)*q4pp + z(218)*q5pp - z(176); - f(3) = z(225) + z(220)*q1pp + z(221)*q2pp + z(222)*q3pp + z(223)*q4pp + z(224)*q5pp - z(177); - f(4) = z(229) + z(210)*q1pp + z(217)*q2pp + z(223)*q3pp + z(227)*q4pp + z(228)*q5pp - z(179); - f(5) = z(231) + z(230)*q5pp + z(212)*q1pp + z(218)*q2pp + z(224)*q3pp + z(228)*q4pp - z(181); - f(6) = L; + f(1) = z(249) + z(239)*q1pp + z(242)*q2pp + z(243)*q3pp + z(246)*q4pp + z(248)*q5pp - z(215); + f(2) = z(255) + z(242)*q1pp + z(251)*q2pp + z(252)*q3pp + z(253)*q4pp + z(254)*q5pp - z(217); + f(3) = z(261) + z(256)*q1pp + z(257)*q2pp + z(258)*q3pp + z(259)*q4pp + z(260)*q5pp - z(218); + f(4) = z(265) + z(246)*q1pp + z(253)*q2pp + z(259)*q3pp + z(263)*q4pp + z(264)*q5pp - z(220); + f(5) = z(267) + z(266)*q5pp + z(248)*q1pp + z(254)*q2pp + z(260)*q3pp + z(264)*q4pp - z(222); df_dq(1,1) = z(469); df_dq(1,2) = z(470); df_dq(1,3) = z(471); @@ -635,21 +745,16 @@ df_dq(3,3) = z(481); df_dq(3,4) = z(482); df_dq(3,5) = z(483); - df_dq(4,1) = -z(448); + df_dq(4,1) = -z(453); df_dq(4,2) = z(484); df_dq(4,3) = z(485); df_dq(4,4) = z(486); df_dq(4,5) = z(488); - df_dq(5,1) = -z(459); + df_dq(5,1) = -z(464); df_dq(5,2) = z(489); df_dq(5,3) = z(490); df_dq(5,4) = z(491); df_dq(5,5) = z(492); - df_dq(6,1) = z(464); - df_dq(6,2) = z(465); - df_dq(6,3) = z(466); - df_dq(6,4) = z(467); - df_dq(6,5) = z(468); df_dqd(1,1) = z(558); df_dqd(1,2) = z(559); df_dqd(1,3) = z(535); @@ -675,47 +780,46 @@ df_dqd(5,3) = z(564); df_dqd(5,4) = z(565); df_dqd(5,5) = 0; - df_dqd(6,1) = 0; - df_dqd(6,2) = 0; - df_dqd(6,3) = 0; - df_dqd(6,4) = 0; - df_dqd(6,5) = 0; - df_dqdd(1,1) = z(203); - df_dqdd(1,2) = z(206); - df_dqdd(1,3) = z(207); - df_dqdd(1,4) = z(210); - df_dqdd(1,5) = z(212); - df_dqdd(2,1) = z(206); - df_dqdd(2,2) = z(215); - df_dqdd(2,3) = z(216); - df_dqdd(2,4) = z(217); - df_dqdd(2,5) = z(218); - df_dqdd(3,1) = z(220); - df_dqdd(3,2) = z(221); - df_dqdd(3,3) = z(222); - df_dqdd(3,4) = z(223); - df_dqdd(3,5) = z(224); - df_dqdd(4,1) = z(210); - df_dqdd(4,2) = z(217); - df_dqdd(4,3) = z(223); - df_dqdd(4,4) = z(227); - df_dqdd(4,5) = z(228); - df_dqdd(5,1) = z(212); - df_dqdd(5,2) = z(218); - df_dqdd(5,3) = z(224); - df_dqdd(5,4) = z(228); - df_dqdd(5,5) = z(230); - df_dqdd(6,1) = 0; - df_dqdd(6,2) = 0; - df_dqdd(6,3) = 0; - df_dqdd(6,4) = 0; - df_dqdd(6,5) = 0; + df_dqdd(1,1) = z(239); + df_dqdd(1,2) = z(242); + df_dqdd(1,3) = z(243); + df_dqdd(1,4) = z(246); + df_dqdd(1,5) = z(248); + df_dqdd(2,1) = z(242); + df_dqdd(2,2) = z(251); + df_dqdd(2,3) = z(252); + df_dqdd(2,4) = z(253); + df_dqdd(2,5) = z(254); + df_dqdd(3,1) = z(256); + df_dqdd(3,2) = z(257); + df_dqdd(3,3) = z(258); + df_dqdd(3,4) = z(259); + df_dqdd(3,5) = z(260); + df_dqdd(4,1) = z(246); + df_dqdd(4,2) = z(253); + df_dqdd(4,3) = z(259); + df_dqdd(4,4) = z(263); + df_dqdd(4,5) = z(264); + df_dqdd(5,1) = z(248); + df_dqdd(5,2) = z(254); + df_dqdd(5,3) = z(260); + df_dqdd(5,4) = z(264); + df_dqdd(5,5) = z(266); df_dFc(1) = -z(566); df_dFc(2) = -z(567); df_dFc(3) = -z(568); df_dFc(4) = -z(569); df_dFc(5) = z(570); - df_dFc(6) = 0; + dL_dq(1,1) = z(193); + dL_dq(1,2) = z(196); + dL_dq(1,3) = -z(194); + dL_dq(1,4) = -z(195); + dL_dq(1,5) = z(197); + dLdot_dq(1,1) = z(608); + dLdot_dq(1,2) = z(629); + dLdot_dq(1,3) = z(658); + dLdot_dq(1,4) = z(672); + dLdot_dq(1,5) = z(678); stick(1,1) = par.Xankle; stick(1,2) = par.Yankle; stick(2,1) = par.Xankle + par.ShankLen*z(1); @@ -734,6 +838,6 @@ df_dq = sparse(df_dq); df_dqd = sparse(df_dqd); df_dqdd = sparse(df_dqdd); - df_dFc = sparse(df_dFc); + df_dFc = transpose(sparse(df_dFc)); end diff --git a/optimizer/makeplots.m b/optimizer/makeplots.m index 984dc38..711107c 100644 --- a/optimizer/makeplots.m +++ b/optimizer/makeplots.m @@ -1,7 +1,7 @@ function makeplots(filename) + global model load(filename); - model = result.model; info = result.info; X = result.X; @@ -39,71 +39,196 @@ function makeplots(filename) ymeas = model.data';% node i T = model.time ; - ankle_angle = sqrt(mean(((X(iy2)' - ymeas(1,:)).^2))); - knee_angle = sqrt(mean(((X(iy3)' - ymeas(2,:)).^2))); - hip_angle = sqrt(mean(((X(iy4)' - ymeas(3,:)).^2))); - shoulder_angle = sqrt(mean(((X(iy5)' - ymeas(4,:)).^2))); - elbow_angle = sqrt(mean(((X(iy6)' - ymeas(5,:)).^2))); - fw_velocity = sqrt(mean(((X(iy7)' - ymeas(6,:)).^2))); - handle_force = sqrt(mean(((X(iy13)' - ymeas(7,:)).^2))); - - fprintf(' ankle_angle error: %8.3f\n\n', ankle_angle); - fprintf(' knee_angle error: %8.3f\n\n', knee_angle); - fprintf(' hip_angle error: %8.3f\n\n', hip_angle); - fprintf(' shoulder_angle error: %8.3f\n\n', shoulder_angle); - fprintf(' elbow_angle error: %8.3f\n\n', elbow_angle); - fprintf(' fw_velocity error: %8.3f\n\n', fw_velocity); - fprintf(' handle_force error: %8.3f\n\n', handle_force); + if model.tracking == 1 + ankle_angle = sqrt(mean(((X(iy2)' - ymeas(1,:)).^2))); + knee_angle = sqrt(mean(((X(iy3)' - ymeas(2,:)).^2))); + hip_angle = sqrt(mean(((X(iy4)' - ymeas(3,:)).^2))); + shoulder_angle = sqrt(mean(((X(iy5)' - ymeas(4,:)).^2))); + elbow_angle = sqrt(mean(((X(iy6)' - ymeas(5,:)).^2))); + elseif model.tracking == 2 + ankle_angle = sqrt(mean(((X(iy2)' - ymeas(1,:)).^2))); + knee_angle = sqrt(mean(((X(iy3)' - ymeas(2,:)).^2))); + hip_angle = sqrt(mean(((X(iy4)' - ymeas(3,:)).^2))); + shoulder_angle = sqrt(mean(((X(iy5)' - ymeas(4,:)).^2))); + elbow_angle = sqrt(mean(((X(iy6)' - ymeas(5,:)).^2))); + handle_force = sqrt(mean(((X(iy13)' - ymeas(6,:)).^2))); + elseif model.tracking == 3 + ankle_angle = sqrt(mean(((X(iy2)' - ymeas(1,:)).^2))); + knee_angle = sqrt(mean(((X(iy3)' - ymeas(2,:)).^2))); + hip_angle = sqrt(mean(((X(iy4)' - ymeas(3,:)).^2))); + shoulder_angle = sqrt(mean(((X(iy5)' - ymeas(4,:)).^2))); + elbow_angle = sqrt(mean(((X(iy6)' - ymeas(5,:)).^2))); + fw_velocity = sqrt(mean(((X(iy7)' - ymeas(6,:)).^2))); + handle_force = sqrt(mean(((X(iy13)' - ymeas(7,:)).^2))); + + elseif model.tracking == 4 + shoulder_angle = sqrt(mean(((X(iy5)' - ymeas(1,:)).^2))); + else + shoulder_angle = sqrt(mean(((X(iy5)' - ymeas(1,:)).^2))); + elbow_angle = sqrt(mean(((X(iy6)' - ymeas(2,:)).^2))); + + end + + + if model.tracking == 1 + fprintf(' ankle_angle error: %8.3f\n\n', ankle_angle); + fprintf(' knee_angle error: %8.3f\n\n', knee_angle); + fprintf(' hip_angle error: %8.3f\n\n', hip_angle); + fprintf(' shoulder_angle error: %8.3f\n\n', shoulder_angle); + fprintf(' elbow_angle error: %8.3f\n\n', elbow_angle); + elseif model.tracking == 2 + fprintf(' ankle_angle error: %8.3f\n\n', ankle_angle); + fprintf(' knee_angle error: %8.3f\n\n', knee_angle); + fprintf(' hip_angle error: %8.3f\n\n', hip_angle); + fprintf(' shoulder_angle error: %8.3f\n\n', shoulder_angle); + fprintf(' elbow_angle error: %8.3f\n\n', elbow_angle); + fprintf(' handle_force error: %8.3f\n\n', handle_force); + + elseif model.tracking == 3 + fprintf(' ankle_angle error: %8.3f\n\n', ankle_angle); + fprintf(' knee_angle error: %8.3f\n\n', knee_angle); + fprintf(' hip_angle error: %8.3f\n\n', hip_angle); + fprintf(' shoulder_angle error: %8.3f\n\n', shoulder_angle); + fprintf(' elbow_angle error: %8.3f\n\n', elbow_angle); + fprintf(' fw_velocity error: %8.3f\n\n', fw_velocity); + fprintf(' handle_force error: %8.3f\n\n', handle_force); + + elseif model.tracking == 4 + fprintf(' shoulder_angle error: %8.3f\n\n', shoulder_angle); + else + fprintf(' shoulder_angle error: %8.3f\n\n', shoulder_angle); + fprintf(' shoulder_angle error: %8.3f\n\n', shoulder_angle); + + + end % ploting joint angles figure(1);clf set(gcf,'renderer','painters'); - subplot(5,1,1); - plot(T, X(iy2)*180/pi,'r', 'LineWidth',2); - hold on; plot(T, ymeas(1,:)*180/pi,'b:' ,'LineWidth',2); - ylabel({'ankle'},'fontweight','bold','fontsize',10) - legend('simulation','data'); - title(' joint angles (rad)') - - subplot(5,1,2); - plot(T, X(iy3)*180/pi,'r', 'LineWidth',2); - hold on; plot(T, ymeas(2,:)*180/pi,'b:' ,'LineWidth',2); - ylabel({'knee'},'fontweight','bold','fontsize',10) - - subplot(5,1,3); - plot(T, X(iy4)*180/pi,'r', 'LineWidth',2); - hold on; plot(T, ymeas(3,:)*180/pi,'b:' ,'LineWidth',2); - ylabel({'hip'},'fontweight','bold','fontsize',10) - - subplot(5,1,4); - plot(T, X(iy5)*180/pi,'r', 'LineWidth',2); - hold on; plot(T, ymeas(4,:)*180/pi,'b:' ,'LineWidth',2); - ylabel({'shoulder'},'fontweight','bold','fontsize',10) - - subplot(5,1,5); - plot(T, X(iy6)*180/pi,'r', 'LineWidth',2); - hold on; plot(T, ymeas(5,:)*180/pi,'b:' ,'LineWidth',2); - ylabel({'elbow'},'fontweight','bold','fontsize',10) - xlabel('time (s)','fontweight','bold','fontsize',10); + if model.tracking == 4 + subplot(5,1,1); + plot(T, X(iy2)*180/pi,'r', 'LineWidth',2); + ylabel({'ankle'},'fontweight','bold','fontsize',10) + legend('simulation','data'); + title(' joint angles (rad)') + + subplot(5,1,2); + plot(T, X(iy3)*180/pi,'r', 'LineWidth',2); + ylabel({'knee'},'fontweight','bold','fontsize',10) + + subplot(5,1,3); + plot(T, X(iy4)*180/pi,'r', 'LineWidth',2); + ylabel({'hip'},'fontweight','bold','fontsize',10) + + subplot(5,1,4); + plot(T, X(iy5)*180/pi,'r', 'LineWidth',2); + ylabel({'shoulder'},'fontweight','bold','fontsize',10) + + subplot(5,1,5); + plot(T, X(iy6)*180/pi,'r', 'LineWidth',2); + hold on; plot(T, ymeas(1,:)*180/pi,'b:' ,'LineWidth',2); + ylabel({'elbow'},'fontweight','bold','fontsize',10) + xlabel('time (s)','fontweight','bold','fontsize',10); + + elseif model.tracking == 5 + subplot(5,1,1); + plot(T, X(iy2)*180/pi,'r', 'LineWidth',2); + ylabel({'ankle'},'fontweight','bold','fontsize',10) + legend('simulation','data'); + title(' joint angles (rad)') + + subplot(5,1,2); + plot(T, X(iy3)*180/pi,'r', 'LineWidth',2); + ylabel({'knee'},'fontweight','bold','fontsize',10) + + subplot(5,1,3); + plot(T, X(iy4)*180/pi,'r', 'LineWidth',2); + ylabel({'hip'},'fontweight','bold','fontsize',10) + + subplot(5,1,4); + plot(T, X(iy5)*180/pi,'r', 'LineWidth',2); + hold on; plot(T, ymeas(1,:)*180/pi,'b:' ,'LineWidth',2); + ylabel({'shoulder'},'fontweight','bold','fontsize',10) + + subplot(5,1,5); + plot(T, X(iy6)*180/pi,'r', 'LineWidth',2); + hold on; plot(T, ymeas(2,:)*180/pi,'b:' ,'LineWidth',2); + ylabel({'elbow'},'fontweight','bold','fontsize',10) + xlabel('time (s)','fontweight','bold','fontsize',10); + else + subplot(5,1,1); + plot(T, X(iy2)*180/pi,'r', 'LineWidth',2); + hold on; plot(T, ymeas(1,:)*180/pi,'b:' ,'LineWidth',2); + ylabel({'ankle'},'fontweight','bold','fontsize',10) + legend('simulation','data'); + title(' joint angles (rad)') + + subplot(5,1,2); + plot(T, X(iy3)*180/pi,'r', 'LineWidth',2); + hold on; plot(T, ymeas(2,:)*180/pi,'b:' ,'LineWidth',2); + ylabel({'knee'},'fontweight','bold','fontsize',10) + + subplot(5,1,3); + plot(T, X(iy4)*180/pi,'r', 'LineWidth',2); + hold on; plot(T, ymeas(3,:)*180/pi,'b:' ,'LineWidth',2); + ylabel({'hip'},'fontweight','bold','fontsize',10) + + subplot(5,1,4); + plot(T, X(iy5)*180/pi,'r', 'LineWidth',2); + hold on; plot(T, ymeas(4,:)*180/pi,'b:' ,'LineWidth',2); + ylabel({'shoulder'},'fontweight','bold','fontsize',10) + + subplot(5,1,5); + plot(T, X(iy6)*180/pi,'r', 'LineWidth',2); + hold on; plot(T, ymeas(5,:)*180/pi,'b:' ,'LineWidth',2); + ylabel({'elbow'},'fontweight','bold','fontsize',10) + xlabel('time (s)','fontweight','bold','fontsize',10); + end + % ploting machine states figure(2); set(gcf,'renderer','painters'); - subplot(3,1,1); - plot(T, X(iy7),'r', 'LineWidth',2); - hold on; plot(T, ymeas(6,:),'b:' ,'LineWidth',2); - ylabel({'flywheel','velocity (m/s)'},'fontweight','bold','fontsize',10) - legend('simulation','data'); - - subplot(3,1,2); - plot(T, X(iy13),'r', 'LineWidth',2); - hold on; plot(T, ymeas(7,:),'b:' ,'LineWidth',2); - ylabel({'force','(kN)'},'fontweight','bold','fontsize',10) - legend('simulation','data'); + + if model.tracking == 2 + subplot(3,1,1); + plot(T, X(iy7),'r', 'LineWidth',2); + ylabel({'flywheel','velocity (m/s)'},'fontweight','bold','fontsize',10) + legend('simulation'); + + subplot(3,1,2); + plot(T, X(iy13),'r', 'LineWidth',2); + hold on; plot(T, ymeas(6,:),'b:' ,'LineWidth',2); + ylabel({'force','(kN)'},'fontweight','bold','fontsize',10) + legend('simulation','data'); + elseif model.tracking == 3 + subplot(3,1,1); + plot(T, X(iy7),'r', 'LineWidth',2); + hold on; plot(T, ymeas(6,:),'b:' ,'LineWidth',2); + ylabel({'flywheel','velocity (m/s)'},'fontweight','bold','fontsize',10) + legend('simulation','data'); + + subplot(3,1,2); + plot(T, X(iy13),'r', 'LineWidth',2); + hold on; plot(T, ymeas(7,:),'b:' ,'LineWidth',2); + ylabel({'force','(kN)'},'fontweight','bold','fontsize',10) + + else + subplot(3,1,1); + plot(T, X(iy7),'r', 'LineWidth',2); + ylabel({'flywheel','velocity (m/s)'},'fontweight','bold','fontsize',10) + legend('simulation'); + + subplot(3,1,2); + plot(T, X(iy13),'r', 'LineWidth',2); + ylabel({'force','(kN)'},'fontweight','bold','fontsize',10) + legend('simulation'); + end + subplot(3,1,3); plot(T, X(iy1)','r', 'LineWidth',2); @@ -135,6 +260,13 @@ function makeplots(filename) ylabel({'elbow'},'fontweight','bold','fontsize',10) xlabel('time (s)','fontweight','bold','fontsize',10); + anklestd = std(X(iu1)'*1000) + kneestd = std(X(iu2)'*1000) + hipstd = std(X(iu3)'*1000) + shoulderstd = std(X(iu4)'*1000) + elbowstd = std(X(iu5)'*1000) + + % ploting cable and rachet machanism figure(4); @@ -155,7 +287,7 @@ function makeplots(filename) q = [X(iy2),X(iy3),X(iy4),X(iy5),X(iy6)]; % extract joint angle trajectories (states 1-5) figure(6); set(gcf,'renderer','painters'); - animate(q,model.parameters); + animate(q,model.parameters); figure(7) diff --git a/optimizer/optimize.m b/optimizer/optimize.m index 2212b96..104a26d 100644 --- a/optimizer/optimize.m +++ b/optimizer/optimize.m @@ -8,183 +8,369 @@ % Weffort....(scalar) weight of effort term in objective % X..........(scalar) initial guess (default: zeros) - global model - load Simresult + global model + load Simresult2 [dSPACEdata, qdata, t, model.parameters] = datainterp2; - T = t(end); %duration of one rowing cycle + T = t(end); %duration of one rowing cycle % decode the problem - N = problem.N; % number of collocation points - model.N = N; - model.tracking = problem.tracking; - model.Wtrack = problem.Wtrack; - model.Weffort = problem.Weffort; - model.h = T/(N-1); % time step for direct collocation - h = model.h; - tdc = (0:h:T)'; % time points for direct collocation - model.Len = zeros(1,model.N); - model.task.Lmin = 0.2077; % min and max of cable lenght from the data - model.task.Lmax = 1.1288; - - % resample the tracking data to the DC time points - qdata = interp1(t, qdata, tdc); - dSPACEdata = interp1(t, dSPACEdata, tdc); - veldata = dSPACEdata(:,1); - fwveldata = dSPACEdata(:,2); - forcedata = dSPACEdata(:,3); - - plot(qdata*180/pi) - title('joint angles') - legend('q1','q2','q3','q4','q5'); - % store the tracking data in global variables - model.time = tdc; -% model.data = [qdata fwveldata forcedata]; % add mocap data (angles) as extra columns - if model.tracking == 1 - model.data = [qdata]; % add mocap data (angles) as extra columns - elseif model.tracking == 2 - model.data = [forcedata]; % add mocap data (angles) as extra columns - else - model.data = [qdata fwveldata forcedata]; % add mocap data (angles) as extra columns - % model.data = [qdata forcedata]; % add mocap data (angles) as extra columns - end - - model.dataSD = std(model.data); - - if model.tracking == 1 + N = problem.N; % number of collocation points + model.N = N; + model.tracking = problem.tracking; + model.Wtrack = problem.Wtrack; + model.Weffort = problem.Weffort; + if isfield(problem,'Reqpower') + model.Reqpower = problem.Reqpower; + end + if isfield(problem,'task') + model.task.Lmin = problem.task.Lmin; + model.task.Lmax = problem.task.Lmax; + end + + + model.h = T/(N-1); % time step for direct collocation + h = model.h; + tdc = (0:h:T)'; % time points for direct collocation + model.Len = zeros(1,model.N); + + % resample the tracking data to the DC time points + qdata = interp1(t, qdata, tdc); + dSPACEdata = interp1(t, dSPACEdata, tdc); + veldata = dSPACEdata(:,1); + fwveldata = dSPACEdata(:,2); + forcedata = dSPACEdata(:,3); + + plot(qdata*180/pi) + title('joint angles') + legend('q1','q2','q3','q4','q5'); + % store the tracking data in global variables + model.time = tdc; + % model.data = [qdata fwveldata forcedata]; % add mocap data (angles) as extra columns + if model.tracking == 1 + model.data = [qdata]; % add mocap data (angles) as extra columns + elseif model.tracking == 2 + model.data = [qdata forcedata]; % add mocap data (angles) as extra columns + elseif model.tracking == 3 + model.data = [qdata fwveldata forcedata]; % add mocap data (angles) as extra columns + elseif model.tracking == 4 + model.data = qdata(:,5); + else + model.data = [qdata(:,4:5)]; + end + + model.dataSD = std(model.data); + + if model.tracking == 1 model.itrack = [2 3 4 5 6]; % the state variables to track (model.itrack=problem.irack so it should go to the script) - elseif model.tracking == 2 - model.itrack = [13]; % the state variables to track (model.itrack=problem.irack so it should go to the script) - else + elseif model.tracking == 2 + model.itrack = [2 3 4 5 6 13]; % the state variables to track (model.itrack=problem.irack so it should go to the script) + elseif model.tracking == 3 model.itrack = [2 3 4 5 6 7 13]; + elseif model.tracking == 4 + model.itrack = [6]; + else + model.itrack = [5 6]; + end + + model.ntrack = numel(model.itrack); + + model.discretization = problem.discretization; % Backward Euler(BE) or Midpoint Euler(ME) discretization + model.N = problem.N; + model.Wtrack = problem.Wtrack; + model.Weffort = problem.Weffort; + model.cgain = problem.cgain; + model.dyngain = problem.dyngain; +% if isfield(problem,'task') +% model.cablecnst = problem.cablecnst; +% else +% model.cablecnst = 0; +% end + model.umax = [ 6.11,6.00,6.071,1.3557,1.1164]'; +% model.umax = [ 1,1,1,1,1]'; + + +% % muscle parameters +% model.L = 0.60; % Length of forearm + hand(m) +% model.d = 0.052; % Moment arm at elbow(m) at 90 degrees +% model.Tact = 10e-3; % Activation time(s) +% model.Tdeact = 40e-3; % Deactivation time(s) +% model.Fmax = 495; % strength of biceps (N) +% model.Lm0 = 0.288; % Muscle-tendon length at 90 deg elbow angle (m) +% +% % muscle properties of the biceps brachii +% model.Fmax = 495; % (N) maximal isometric force (sum of both parts of the muscle) +% model.umax = 0.033; % (dimensionless) strain in the series elastic element at load of Fmax +% model.W = 0.63; % (dimensionless) width parameter of the force-length relationship of the contractile element +% model.AHill = 0.25; % (dimensionless) Hill parameter of the force-velocity relationship +% model.FVmax = 1.5; % (dimensionless) maximal eccentric force +% model.Lceopt = 0.1106; % (m) optimal CE length (MUST BE AVERAGE OF THE TWO PARTS OF THE BICEPS) +% model.b = 0.01; % (s/m) damping coefficient of damper parallel to the CE (normalized to Fmax) +% model.SEELslack = 0.192; % (m) slack length of the series elastic element +% model.PEELslack = 1; % (dimensionless) slack length of the parallel elastic element, divided by Lceopt + +% % constants derived from the muscle properties +% model.Vmax = 10*model.Lceopt; % Maximum shortening velocity (m/s) is 10 fiber lengths per sec +% model.d1 = model.Vmax*model.AHill*(model.FVmax-1)/(model.AHill+1); % parameter in the eccentric force-velocity equation +% +% % constant force which is always exists in both phases to balance out +% % the muscle constant force (even if the amplitude is zero, muscle generates force) +% model.constantforce = 10; % (N) constant force added to k2 +% +% % Since the model parameters are obtained from parameters +% % identification of a Concep-2 rowing machine, they have to be scaled +% % down to use in this model +% % So the model parameters should be multiplied by scale factor +% +% % the force scale factor(SF) = maximal pull force based on biceps +% % muscle strength / maximal force from Kleshnev paper +% % maximum force in Kleshnev data is 800 N +% FSF = ((model.d/model.L)*model.Fmax)/(800); % force scale factor +% +% % The displacement scale factor(SD) = amplitude / maximum displacement +% % in Kleshnev test +% % maximum displacement in Kleshnev data is 1.35 m +% DSF = 0.4/(1.35); % distance scale factor +% +% % Scale Factor = force scale factor/distance scale factor +% SF = FSF/DSF; +% +% % So stiffness parameters, linear damping stiffness and masses should +% % be multiplied by (SF) +% % The quadratic damping parameter should be multiplied by (FSF/DSF^2) +% + % define the state variables x, controls u, and their bounds + % x = [x_fw, q1 ...q5, v_fw, qd1...qd5, F(kn)] + % u = [M1(knm)...M5]; + model.nx = 13; + model.nu = 5; + model.np = problem.np; + + %before scaling: + % x_lb = [0 0 -2*pi 0 -pi/2 0 0 -10 -10 -10 -10 -10 0]'; + % x_ub = [3 2*pi 2*pi 2*pi pi/3 2*pi 20 20 20 20 20 20 500 ]'; + % u_lb = [-700; -700 ;-700 ;-700 ;-700]; + % u_ub = [700 ;700 ;700 ;700 ;700]; + + % when scaled + % x_lb = [0 0 -2*pi 0 -pi/2 0 0 -20 -10 -10 -10 -10 0 ]'; + % x_ub = [3 2*pi 2*pi 2*pi pi/3 2*pi 20 20 20 20 20 20 5 ]'; + x_lb = [0 80*pi/180 20*pi/180 50*pi/180 -40*pi/180 10*pi/180 0 -20 -10 -10 -10 -10 0 ]'; + x_ub = [3 160*pi/180 130*pi/180 120*pi/180 80*pi/180 115*pi/180 20 20 20 20 20 20 0.42 ]'; + u_lb = [-700; -700 ;-700 ;-700 ;-700]/1000; + u_ub = [700 ;700 ;700 ;700 ;700]/1000; + + if model.Wtrack ==0 + u_lb = [-100; -60 ;-50 ;-20 ;-20]/1000; + u_ub = [100 ;60;150 ;10 ;20]/1000; + + end + + + if isfield(model,'task') + p_lb = 0.5; + p_ub = 1.4; +% p_lb = 1; +% p_ub = 1; + end + + % make bounds for the trajectory optimization problem + if isfield(model,'task') + + X_lb = [repmat([x_lb ; u_lb], N, 1);p_lb]; + X_ub = [repmat([x_ub ; u_ub], N, 1);p_ub]; + X_lb(1) = model.task.Lmin; + X_ub(1) = model.task.Lmin; + else + X_lb = [repmat([x_lb ; u_lb], N, 1)]; + X_ub = [repmat([x_ub ; u_ub], N, 1)]; + end + + + + % all variables, except flywheel position, must be periodic + model.iper = 2:model.nx; + if isfield(model,'task') + model.npar = numel(p_lb); + else + model.npar = 0; + end + model.nper = numel(model.iper); +% if isfield(model,'task') +% +% model.ntask = problem.ntask; % sum of the task and path constaraint +% % model.ntask = 2; % number of constraints for task when doing predictive simulation (cable length) Lmin & Lmax & Xwrist for all model.N-1 nodes +% else +% model.ntask = 0; +% end + + %model.ntask = 3; % number of constraints for task when doing predictive simulation (intial and final cable length + + %flywheel postion should be equal to the max cable length at tmax + intial flywheel postion should be equal to the intial cable length ) + + % collocation grid and unknowns + model.Nvarpernode = model.nx + model.nu ; % number of unknowns per node + model.Nconpernode = model.nx ; % number of constraint equations per node (dynamic constraints) + model.Nvar = N * model.Nvarpernode + model.npar ; % number of unknowns + model.ncondyn = (N-1)* model.Nconpernode; + % task constraints included periodicity and cable length and path constraints + model.ncontaskper = model.nu + model.nper; % task : periodicty contraints +% if isfield(problem,'task') +% model.cablecnst = problem.cablecnst; +% else +% model.cablecnst = 0; +% end +% model.ncontaskcbl = problem.cablecnst; % task : cable constraints + if isfield(problem,'task') + model.nconpath = problem.nconpath ; + model.ncontaskcbl = problem.cablecnst; + else + model.nconpath = 0; + model.ncontaskcbl = 0; + end + % path constraints + + model.ntask = model.ncontaskcbl + model.nconpath ; % sum of cable and path constraints + + model.Ncon = model.ncondyn + model.ncontaskper + model.ntask; % number of constrains (including state and control periodicity) + if isfield(problem,'Reqpower') + model.Ncon = model.ncondyn + model.ncontaskper + model.ntask + 1; + end + if isfield(model,'task') + model.ip = model.N*model.Nvarpernode + (1:model.npar); % since we only have 1 parameter, Npar = 1 + else + model.ip = model.N*model.Nvarpernode; % since we only have 1 parameter, Npar = 1 + end + + + + % if oldresult was provided, use it as initial guess, otherwise use data as initial guess + if isfield(problem,'initialguess') +% load(problem.initialguess)%look at the contend of that + if isfield(model,'task') % if optimizing the time inwhich the cable length is in its max + if ischar(problem.initialguess) + %adjusting the data (Simresult2) based on number of collocation nodes(N=result.model.N) + Simtemp = reshape(Simresult2,18,70); + Simtemp=Simtemp'; + htemp = T/(70-1); % time step for direct collocation with 70 nodes + tdctemp = (0:htemp:T)'; % time points for direct collocation with 70 nodes + for j=1:18 + Sim(:,j) = interp1(tdctemp,Simtemp(:,j),tdc); + end + Sim=Sim'; + Simresult2 = Sim(:); +% + + if strcmp(problem.initialguess,'random') + X0 = [Simresult2;1.052]+0.1*rand(model.Nvar,1); + % X0 = X_lb + (X_ub - X_lb).*rand(model.Nvar,1); + elseif strcmp(problem.initialguess,'zero') + X0 = zeros(1261,1); + elseif strcmp(problem.initialguess,'midpoint') + X0 = (X_lb + X_ub)/2; + else + X0 = [Simresult2;1.0809]; + % ix = 1:model.nx; + % for i = 1:model.N-1 + % iy = ix(model.itrack); + % X0(iy) = model.data(i,:)'; % node i + % ix = ix + model.Nvarpernode; + % end + end + + else + X0 = zeros(numel(X_lb),1); + end + else + + if ischar(problem.initialguess) % if tmax was not included in optimization + if strcmp(problem.initialguess,'random') + X0 = 0.1*rand(model.Nvar,1); + elseif strcmp(problem.initialguess,'zero') + X0 = zeros(numel(X_lb),1); + elseif strcmp(problem.initialguess,'midpoint') + X0 = (X_lb + X_ub)/2; + else + X0 = [Simresult2]; + + end + + else + X0 = zeros(numel(X_lb),1); + end + end + end - end - - model.ntrack = numel(model.itrack); - - model.discretization = problem.discretization; % Backward Euler(BE) or Midpoint Euler(ME) discretization - model.N = problem.N; - model.Wtrack = problem.Wtrack; - model.Weffort = problem.Weffort; - - - % define the state variables x, controls u, and their bounds - % x = [x_fw, q1 ...q5, v_fw, qd1...qd5, F(kn)] - % u = [M1(knm)...M5]; - model.nx = 13; - model.nu = 5; - model.np = 1; - - %before scaling: - % x_lb = [0 0 -2*pi 0 -pi/2 0 0 -10 -10 -10 -10 -10 0]'; - % x_ub = [3 2*pi 2*pi 2*pi pi/3 2*pi 20 20 20 20 20 20 500 ]'; - % u_lb = [-700; -700 ;-700 ;-700 ;-700]; - % u_ub = [700 ;700 ;700 ;700 ;700]; - - % when scaled -% x_lb = [0 0 -2*pi 0 -pi/2 0 0 -20 -10 -10 -10 -10 0 ]'; -% x_ub = [3 2*pi 2*pi 2*pi pi/3 2*pi 20 20 20 20 20 20 5 ]'; - x_lb = [0 80*pi/180 20*pi/180 50*pi/180 -40*pi/180 10*pi/180 0 -20 -10 -10 -10 -10 0 ]'; - x_ub = [3 160*pi/180 130*pi/180 120*pi/180 80*pi/180 115*pi/180 20 20 20 20 20 20 0.5 ]'; - u_lb = [-700; -700 ;-700 ;-700 ;-700]/1000; - u_ub = [700 ;700 ;700 ;700 ;700]/1000; - p_lb = 0.5; - p_ub = 1.5; -% p_lb = 0.7; -% p_ub = 1.5; - % make bounds for the trajectory optimization problem - X_lb = [repmat([x_lb ; u_lb], N, 1);p_lb]; - X_ub = [repmat([x_ub ; u_ub], N, 1);p_ub]; - X_lb(1) = model.task.Lmin; - X_ub(1) = model.task.Lmin; - - - + - % all variables, except flywheel position, must be periodic - model.iper = 2:model.nx; - model.npar = numel(p_lb); - model.nper = numel(model.iper); - model.ntask = 2; % number of constraints for task when doing predictive simulation (cable length) - % collocation grid and unknowns - model.Nvarpernode = model.nx + model.nu ; % number of unknowns per node - model.Nconpernode = model.nx ; % number of constraint equations per node (number of dynamic equations) - model.Nvar = N * model.Nvarpernode + model.npar ; % number of unknowns - model.Ncon = (N-1)* model.Nconpernode + model.nu + model.nper + model.ntask; % number of constrains (including state and control periodicity) - model.ip = model.N*model.Nvarpernode + (1:model.npar); % since we only have 1 parameter, Npar = 1 - - % if oldresult was provided, use it as initial guess, otherwise use data as initial guess - if isfield(problem,'initialguess') - if ischar(problem.initialguess) - if strcmp(problem.initialguess,'random') - X0 = [Simresult;1.052]+0.1*rand(model.Nvar,1); -% X0 = X_lb + (X_ub - X_lb).*rand(model.Nvar,1); - - elseif strcmp(problem.initialguess,'zero') - X0 = X_lb + (X_ub - X_lb).*rand(model.Nvar,1); - elseif strcmp(problem.initialguess,' midpoint') - X0 = (X_lb + X_ub)/2; - else - X0 = [Simresult;1.052]; -% ix = 1:model.nx; -% for i = 1:model.N-1 -% iy = ix(model.itrack); -% X0(iy) = model.data(i,:)'; % node i -% ix = ix + model.Nvarpernode; -% end - end - - else - X0 = zeros(numel(X_lb),1); + % Determine Jacobian structure and number of non-zeros + Xr = rand(model.Nvar,1); + % Xr(end) = 1.0519; + model.Jnnz = 1; + J = conjac(Xr); %determins where the jacobian is not zoro + model.conjacstructure = double(J ~= 0);% it compares each element in the jacobian to zero(it makes it one when its not zero, and zero when its zero) + %and then it stores it in Matlab as a logical variable (binary + %variable). (recording 4 at 1:00:00) + model.Jnnz = nnz(J); + model.conjacstructure(end-1:end,:) = 1; + + % Check the derivations, if N=10 was specified + if N == 4 - end - end + checkderiv; % compare gradient and Jacobian against finite difference approximations + end + + % solve the NLP with IPOPT + funcs.objective = @objfun ; + funcs.gradient = @objgrad; + funcs.constraints = @confun ; + funcs.jacobian = @conjac ; + funcs.jacobianstructure = @conjacstructure; + + options.lb = X_lb; + options.ub = X_ub; + options.cl = zeros(model.Ncon,1); + options.cu = zeros(model.Ncon,1); + % if model.nconpath == model.N + if model.nconpath>0 + +% options.cu(model.ncondyn + model.ncontaskper + model.ncontaskcbl +( 1:model.nconpath) ) = 1.5*model.cgain; +% options.cl(model.ncondyn + model.ncontaskper + model.ncontaskcbl +( 1:model.nconpath)) = 0.78*model.cgain; + options.cu(model.ncondyn + model.ncontaskper + model.ncontaskcbl +(1) ) = 1.5*model.cgain; + options.cl(model.ncondyn + model.ncontaskper + model.ncontaskcbl +(1)) = 0.78*model.cgain; + if model.nconpath==2 + options.cu(model.ncondyn + model.ncontaskper + model.ncontaskcbl +(2) ) = 1.5*model.cgain; + options.cl(model.ncondyn + model.ncontaskper + model.ncontaskcbl +(2)) = 0.78*model.cgain; + end + % options.cu(model.ncondyn + model.ncontaskper + model.ncontaskcbl +( 1:model.nconpath) ) = model.cgain*[0.984587124166634,0.949239382211693,0.940856786460101,0.945733510058762,0.957084562150634,0.972721713834140,0.982509343643580,0.987816947672726,0.990539007860171,0.990227649806674,0.994967362720877,1.00263477046094,1.00334222046470,1.00144009367118,0.997344919254536,0.989714675158803,0.986123385439148,0.977004790375032,0.968045821421840,0.958646052162770,0.948193316336059,0.941418405647705,0.938910094583808,0.924573210865546,0.906861858588330,0.892478320365616,0.881124752664981,0.912010855669652,0.931751477873481,0.944534048076951,0.949628794333322,0.949838966830169,0.948059476679608,0.945912702743256,0.939938348258617,0.930775839376757,0.923109611948986,0.912706410950738,0.907525287589332,0.913178747383131,0.918094181330246,0.925448288060352,0.931908885377663,0.938841859848728,0.947657130474933,0.958560954984599,0.970764124781298,0.984569343535761,0.991153597130201,0.999707630343451,1.00771863848097,1.01451724929020,1.01957971015749,1.02292890754400,1.02408734776336,1.02335939940512,1.02101448675558,1.01703412767700,1.01073978538003,1.00238201602046,0.991859778375628,0.980683771690154,0.972870044715305,0.971611739892830,0.969269605275424,0.965862830792782,0.961891542922626,0.952998931433828,0.948379405406072,0.984587124166634]'; + % options.cl(model.ncondyn + model.ncontaskper + model.ncontaskcbl +( 1:model.nconpath) ) = model.cgain*[0.984587124166634,0.949239382211693,0.940856786460101,0.945733510058762,0.957084562150634,0.972721713834140,0.982509343643580,0.987816947672726,0.990539007860171,0.990227649806674,0.994967362720877,1.00263477046094,1.00334222046470,1.00144009367118,0.997344919254536,0.989714675158803,0.986123385439148,0.977004790375032,0.968045821421840,0.958646052162770,0.948193316336059,0.941418405647705,0.938910094583808,0.924573210865546,0.906861858588330,0.892478320365616,0.881124752664981,0.912010855669652,0.931751477873481,0.944534048076951,0.949628794333322,0.949838966830169,0.948059476679608,0.945912702743256,0.939938348258617,0.930775839376757,0.923109611948986,0.912706410950738,0.907525287589332,0.913178747383131,0.918094181330246,0.925448288060352,0.931908885377663,0.938841859848728,0.947657130474933,0.958560954984599,0.970764124781298,0.984569343535761,0.991153597130201,0.999707630343451,1.00771863848097,1.01451724929020,1.01957971015749,1.02292890754400,1.02408734776336,1.02335939940512,1.02101448675558,1.01703412767700,1.01073978538003,1.00238201602046,0.991859778375628,0.980683771690154,0.972870044715305,0.971611739892830,0.969269605275424,0.965862830792782,0.961891542922626,0.952998931433828,0.948379405406072,0.984587124166634]'; + % + %917-986 + end + +% model.cl=options.cl ; +% model.cu=options.cu; - + % options.cl(end-model.ntask+2) = 10; + % options.cu(end-model.ntask+2) = 0; - % Determine Jacobian structure and number of non-zeros - Xr = rand(model.Nvar,1); -% Xr(end) = 1.0519; - model.Jnnz = 1; - J = conjac(Xr); %determins where the jacobian is not zoro - model.conjacstructure = double(J ~= 0);% it compares each element in the jacobian to zero(it makes it one when its not zero, and zero when its zero) - %and then it stores it in Matlab as a logical variable (binary - %variable). (recording 4 at 1:00:00) - model.Jnnz = nnz(J); - model.conjacstructure(end-1:end,:) = 1; - - % Check the derivations, if N=10 was specified - if N == 4 - checkderiv; % compare gradient and Jacobian against finite difference approximations - end - - % solve the NLP with IPOPT - funcs.objective = @objfun ; - funcs.gradient = @objgrad; - funcs.constraints = @confun ; - funcs.jacobian = @conjac ; - funcs.jacobianstructure = @conjacstructure; - - options.lb = X_lb; - options.ub = X_ub; - options.cl = zeros(model.Ncon,1); - options.cu = zeros(model.Ncon,1); - options.ipopt.max_iter = 5000; - options.ipopt.hessian_approximation = 'limited-memory'; - options.ipopt.tol = 1e-3; - % options.ipopt.print_level = 0; - - [X, info] = ipopt(X0,funcs,options); - - if info.status ~= 0 - error('IPOPT did not solve'); - end - - % create the structure containing the results (using the same result structure from the previous optimization) - result.model = model; - result.X = X; - result.info = info; + options.ipopt.max_iter = 5000; +% options.ipopt.hessian_approximation = 'limited-memory'; + options.ipopt.hessian_approximation = 'limited-memory'; + +options.ipopt.tol = 1e-3; + % options.ipopt.print_level = 0; + + [X, info] = ipopt(X0,funcs,options); + + if info.status ~= 0 + warning('IPOPT did not solve'); + end + + % create the structure containing the results (using the same result structure from the previous optimization) + result.model = model; + result.X = X; + result.info = info; @@ -211,10 +397,16 @@ f2 = 0; iu = model.nx + (1:model.nu); +% umax = [5,5,5,0.4,0.2]'; +% umax = [1,1,1,0.4,0.2]'; +% umax = [1,1,2,0.4,0.6]'; + +% umax = [1,1,1,1,1]'; +% umax = [2.89,1.18,6.97,1.09,0.57]'; for i = 1:model.N-1 u = X(iu); - f2 = f2 + mean(u.^2)*1000; + f2 = f2 + mean((u./model.umax).^2)*1000; iu = iu + model.Nvarpernode; end % iw = 8:12; @@ -243,6 +435,14 @@ % gradient of tracking cost ix = 1:model.nx; +% umax = [5,5,3,0.4,0.2]'; +% umax = [1,1,1,1,1]'; +% umax = [2.89,1.18,6.97,1.09,0.57]'; +% umax = [5,5,5,0.4,0.2]'; +% umax = [1,1,1,0.4,0.2]'; +% umax = [1,1,2,0.4,0.6]'; + + for i = 1:model.N-1 iy = ix(model.itrack)'; y = X(iy); @@ -261,7 +461,7 @@ for i = 1:model.N-1 u = X(iu); % f2 = f2 + mean(u.^2); - g(iu) = g(iu) + model.Weffort * 2*u *1000; + g(iu) = g(iu) + model.Weffort * 2*u*1000./(model.umax.^2) ; g(iu)=g(iu)/(model.N-1)/model.nu; %g(iu)=g(iu)/model.nu; iu = iu + model.Nvarpernode; @@ -315,7 +515,22 @@ % the difference is we have to have seperate x and u h = model.h; + load('Cons.mat') c = zeros(model.Ncon,1); % initialize the constraints +% c = Cons; % initialize the constraints + + if model.nconpath == 2 + c = [Cons;Cons(end)]; +% c(model.ncondyn + model.ncontaskper + model.ncontaskcbl+1) = 0.8014; +% c(model.ncondyn + model.ncontaskper + model.ncontaskcbl+2) = 0.7866; + end + + par.Yankle = 0.5182; + par.ForearmLen = 0.2355; + par.ShankLen = 0.4252; + par.ThighLen = 0.4009; + par.UpparmLen = 0.2856; + par.TrunkLen = 0.4312; % dynamics constraints ix1 = 1:model.nx; % index for the state variables of node 1 @@ -336,9 +551,9 @@ % use Backward Euler formula or Midpoint Euler formula as dynamics constraint if strcmp(model.discretization, 'BE') - c(ic) = dynfun(x2 , (x2-x1)/h, u2); + c(ic) = dynfun(x2 , (x2-x1)/h, u2)/model.dyngain; else - c(ic) = dynfun((x1+x2)/2 , (x2-x1)/h, (u1+u2)/2); + c(ic) = dynfun((x1+x2)/2 , (x2-x1)/h, (u1+u2)/2)/model.dyngain; end ix1 = ix1 + model.Nvarpernode; @@ -354,39 +569,129 @@ npercon = numel(ifirst); c(model.Nconpernode*(model.N-1)+(1:npercon)) = ... - X(ifirst) - X(ilast); % node one - the last one + X(ifirst) - X(ilast); % node one - the last one 989-914 -% % task constraint for the min cable length - x0 = X(1:model.nx); - [~,~,~,~,L0] = dynfun(x0,zeros(model.nx,1),zeros(model.nu,1)); % the cable length L only depends the body positions (q) - c(end-model.ntask+1) = L0 - model.task.Lmin; % difference between final and initial cable length c(end-1) -% - % task constraint for the max cable length - tmax = X(end-model.npar+1); - i = find(min(diff(tmax > model.time)) == diff(tmax > model.time)); % find the index of Model.time which is just less than tmax: Huawei - f = (tmax-model.time(i))/(model.time(i+1)-model.time(i)); % - xi1 = X( (model.nx+model.nu)*(i-1) + (1:model.nx) ) ; % the state x at node i - [~,~,~,~,Li1] = dynfun(xi1,zeros(model.nx,1),zeros(model.nu,1)); % find the max cable length first point - xi2 = X( (model.nx+model.nu)*i + (1:model.nx)) ; % the state x at node i+1 - [~,~,~,~,Li2] = dynfun(xi2,zeros(model.nx,1),zeros(model.nu,1)); % find the max cable length second point - Ltmax = f*Li1 + (1-f)*Li2 ; % linear interpolation of two above nodes - c(end) = Ltmax - model.task.Lmax; % difference between final and initial cable length - -d - - - - + if isfield(model,'task') + % task constraint for the min cable length + x0 = X(1:model.nx); + [~,~,~,~,L0] = dynfun(x0,zeros(model.nx,1),zeros(model.nu,1)); % the cable length L only depends the body positions (q) + c(model.ncondyn + model.ncontaskper + 1) = L0 - model.task.Lmin; % difference between final and initial cable length c(model.Ncon-1) + + + % task constraint for the max cable length + tmax = X(end-model.npar+1); + i = find(min(diff(tmax > model.time)) == diff(tmax > model.time)); % find the index of Model.time which is just less than tmax: Huawei + f = (tmax-model.time(i))/(model.time(i+1)-model.time(i)); % + xi1 = X( (model.nx+model.nu)*(i-1) + (1:model.nx) ) ; % the state x at node i + [~,~,~,~,Li1] = dynfun(xi1,zeros(model.nx,1),zeros(model.nu,1)); % find the max cable length first point + xi2 = X( (model.nx+model.nu)*i + (1:model.nx)) ; % the state x at node i+1 + [~,~,~,~,Li2] = dynfun(xi2,zeros(model.nx,1),zeros(model.nu,1)); % find the max cable length second point + + Ltmax = (1-f)*Li1 + f*Li2 ; % linear interpolation of two above nodes + c(model.ncondyn + model.ncontaskper + model.ncontaskcbl) = Ltmax - model.task.Lmax; % difference between maximum cable length from the previous simulation results and maximum cable length + + if model.nconpath>0 + % for i = 1:model.N + % for i = 1:model.nconpath + i = 35; + + q1= X((model.nx+model.nu)*(i-1)+2); + q2= X((model.nx+model.nu)*(i-1)+3); + q3= X((model.nx+model.nu)*(i-1)+4); + q4= X((model.nx+model.nu)*(i-1)+5); + q5= X((model.nx+model.nu)*(i-1)+6); + % q = [q1,q2,q3,q4,q5]';%trasnpose + % % + % qddot = zeros(5,1); + % qdot = zeros(5,1); + % + % % F(:,i) = X((model.nx+model.nu)*(i-1)+13);%zero for this also + % F = 0; + % + % [~,~,~,~,~,~,~,~,stick] = rowerdynamics(q,qdot,qddot,F,model.parameters); + % wrist(i) = stick (6,2)';%keep the wrist higher than a constant value(so we dont need to use hip) + % wristXmine(i) = par.Yankle - par.ShankLen*sin(-q1(i)) +par.ThighLen*sin(q2(i)+q1(i))+... + % par.TrunkLen*sin(-q2(i)-q1(i)-q3(i))- par.UpparmLen*sin(-q2(i)-q1(i)-q3(i)-q4(i))+... + % par.ForearmLen*sin(+q2(i)+q1(i)+q3(i)+q4(i)+q5(i)); + % calculate the vertical cordinate of the wrist + c(model.ncondyn + model.ncontaskper + model.ncontaskcbl+1) = (par.Yankle + par.ShankLen*sin(pi-q1) -... + par.ThighLen*sin(q2-pi+q1)+... + par.TrunkLen*sin(-q2-q1-q3+2*pi)-... + par.UpparmLen*sin(-q2-q1-q3-q4+2*pi)+... + par.ForearmLen*sin(-2*pi+q2+q1+q3+q4+q5))*model.cgain; + + + if model.nconpath==2 + j = 36; + + q1= X((model.nx+model.nu)*(j-1)+2); + q2= X((model.nx+model.nu)*(j-1)+3); + q3= X((model.nx+model.nu)*(j-1)+4); + q4= X((model.nx+model.nu)*(j-1)+5); + q5= X((model.nx+model.nu)*(j-1)+6); + c(model.ncondyn + model.ncontaskper + model.ncontaskcbl+2) = (par.Yankle + par.ShankLen*sin(pi-q1) -... + par.ThighLen*sin(q2-pi+q1)+... + par.TrunkLen*sin(-q2-q1-q3+2*pi)- ... + par.UpparmLen*sin(-q2-q1-q3-q4+2*pi)+... + par.ForearmLen*sin(-2*pi+q2+q1+q3+q4+q5))*model.cgain; + + end + end + % Ltmax = (1-f)*Li1 + f*Li2 ; % linear interpolation of two above nodes + % c(end-model.ntask+2) = Ltmax - model.task.Lmax; % difference between maximum cable length from the previous simulation results and maximum cable length + end + +% %task constraint for the flywheel position (in case of using both costraint for flywheel position model.ntask=4 +% %or if using just one of them model.ntask=3 ) +% flywheel_pos1 = xi1(1); +% flywheel_pos2 = xi2(1); +% flywheel_pos = f*flywheel_pos1 + (1-f)*flywheel_pos2; +% c(end-model.ntask+3) = flywheel_pos - model.task.Lmax; % difference between maximum cable length and flwyheel position + + if isfield(model,'Reqpower') + + ivel =7:model.nx+model.nu:(model.N-2)*(model.nx+model.nu)+7; + %nvarpernode + iF = 13:model.nx+model.nu:(model.N-2)*(model.nx+model.nu)+13; + V_fw = X(ivel); + F = X(iF); + c(model.ncondyn + model.ncontaskper + model.ncontaskcbl+model.nconpath+1) = sum(V_fw .* F) / (model.N-1) - model.Reqpower; + end + model.c=c; + plot(model.c) + -end + + + end + + +% a=c(917:model.Ncon); +% plot(c,'r');hold on;plot(model.cl,'b');hold on;plot(model.cu,'g') +% end %==================================================================== function [J] = conjac(X) global model - h = model.h; +% c = zeros(model.Ncon,1); % initialize the constraints + load('Cons.mat') c = zeros(model.Ncon,1); % initialize the constraints +% c = Cons; % initialize the constraints + +% if model.nconpath == 2 +% c = [Cons;Cons(end)]; +% % c(model.ncondyn + model.ncontaskper + model.ncontaskcbl+1) = 0.8014; +% % c(model.ncondyn + model.ncontaskper + model.ncontaskcbl+2) = 0.7866; +% end + par.Yankle = 0.5182; + par.ForearmLen = 0.2355; + par.ShankLen = 0.4252; + par.ThighLen = 0.4009; + par.UpparmLen = 0.2856; + par.TrunkLen = 0.4312; + % initialize the sparse Jacobian matrix J = spalloc(model.Ncon,model.Nvar, model.Jnnz); @@ -407,16 +712,17 @@ p = X(model.ip); if strcmp(model.discretization, 'BE') - [~, dfdx, dfdxdot,dfdu] = dynfun(x2, (x2-x1)/h, u2); %?????dLdq - J(ic,ix1) = -dfdxdot/h; - J(ic,ix2) = dfdx + dfdxdot/h; - J(ic, iu2) = dfdu; + [~, dfdx, dfdxdot,dfdu]= dynfun(x2, (x2-x1)/h, u2); %?????dLdq + + J(ic,ix1) = -dfdxdot/h/model.dyngain; + J(ic,ix2) = (dfdx + dfdxdot/h)/model.dyngain; + J(ic, iu2) = dfdu/model.dyngain; else [~, dfdx, dfdxdot,dfdu] = dynfun((x1+x2)/2, (x2-x1)/h, (u1+u2)/2); %?????dLdq - J(ic,ix1) = dfdx/2 - dfdxdot/h; - J(ic,ix2) = dfdx/2 + dfdxdot/h; - J(ic, iu1) = dfdu/2; - J(ic, iu2) = dfdu/2; + J(ic,ix1) = (dfdx/2 - dfdxdot/h)/model.dyngain; + J(ic,ix2) = (dfdx/2 + dfdxdot/h/model.dyngain); + J(ic, iu1) = dfdu/2/model.dyngain; + J(ic, iu2) = dfdu/2/model.dyngain; end % advance ix1 and irow to next node @@ -428,7 +734,7 @@ end % periodicity constraints for states and controls - % dynamics constraints: state periodicity (all variables axcept state 1)and conbtrol periodicity + % state periodicity (all variables axcept state 1)and conbtrol periodicity ifirst = [model.iper model.nx+(1:model.nu)]; ilast = (model.N-1)*model.Nvarpernode + ifirst; npercon = numel(ifirst); @@ -438,36 +744,124 @@ J(model.Nconpernode*(model.N-1)+(1:npercon) , ifirst) = speye(npercon); J(model.Nconpernode*(model.N-1)+(1:npercon) , ilast) = -speye(npercon); - % task constraint for the min cable length - x0 = X(1:model.nx); - [~,~,~,~,L0,dL0dq] = dynfun(x0,zeros(model.nx,1),zeros(model.nu,1)); % the cable length L only depends the body positions (q) - c(end-model.ntask+1) = L0 - model.task.Lmin; % difference between initial cable length and the min cable length from the data - J(end-model.ntask+1 , 2:6 ) = dL0dq; % L0 is a function of qs (2-6) -% - % task constraint for the max cable length - tmax = X(end-model.npar+1); - i = find( min(diff(tmax > model.time)) == diff(tmax > model.time) ); % find the index of Model.time which is just less than tmax: Huawei - - f = (tmax-model.time(i))/(model.time(i+1)-model.time(i)); % - - xi1 = X( (model.nx+model.nu)*(i-1) + (1:model.nx) ) ; % the state x at node i - [~,~,~,~,Li1,dLi1dt] = dynfun(xi1,zeros(model.nx,1),zeros(model.nu,1)); % find the max cable length first point - - xi2 = X( (model.nx+model.nu)*i + (1:model.nx)) ; % the state x at node i+1 - [~,~,~,~,Li2,dLi2dt] = dynfun(xi2,zeros(model.nx,1),zeros(model.nu,1)); % find the max cable length second point - - Ltmax = f*Li1 + (1-f)*Li2 ; % linear interpolation of two above nodes - c(end) = Ltmax - model.task.Lmax; % difference between final and initial cable length + if isfield(model,'task') + + % task constraint for the min cable length + x0 = X(1:model.nx); + [~,~,~,~,L0,dL0dq] = dynfun(x0,zeros(model.nx,1),zeros(model.nu,1)); % the cable length L only depends the body positions (q) + % c((model.ncondyn + model.ncontaskper + 1) = L0 - model.task.Lmin; % difference between initial cable length and the min cable length from the data + J(model.ncondyn + model.ncontaskper + 1 , 2:6 ) = dL0dq; % L0 is a function of qs (2-6) + + % task constraint for the max cable length + tmax = X(end-model.npar+1); + % i = find( min(diff(tmax > model.time)) == diff(tmax > model.time) ); % find the index of Model.time which is just less than tmax: Huawei + i = find( -1 == diff(tmax > model.time) ); % find the index of Model.time which is just less than tmax: Huawei + + f = (tmax-model.time(i))/(model.time(i+1)-model.time(i)); % + xi1 = X( (model.nx+model.nu)*(i-1) + (1:model.nx) ) ; % the state x at node i + [~,~,~,~,Li1,dLi1dt] = dynfun(xi1,zeros(model.nx,1),zeros(model.nu,1)); % find the max cable length first point + xi2 = X( (model.nx+model.nu)*i + (1:model.nx)) ; % the state x at node i+1 + [~,~,~,~,Li2,dLi2dt] = dynfun(xi2,zeros(model.nx,1),zeros(model.nu,1)); % find the max cable length second point + % Ltmax = (1-f)*Li1 + f*Li2 ; % linear interpolation of two above nodes + % c(end-model.ntask+2) = Ltmax - model.task.Lmax; % difference between maximum cable length from the previous simulation results and maximum cable length + + J(model.ncondyn + model.ncontaskper + model.ncontaskcbl, (model.nx+model.nu)*(i-1) + (2:6)) = (1-f)* dLi1dt ; + + J(model.ncondyn + model.ncontaskper + model.ncontaskcbl, (model.nx+model.nu)*i + (2:6)) = f*dLi2dt; + dLtmaxdf = -Li1 + Li2; + dfdtmax = 1/(model.time(i+1)-model.time(i)); + J(model.ncondyn + model.ncontaskper + model.ncontaskcbl, end) = dLtmaxdf*dfdtmax; + + + if model.nconpath>0 + + % for i = 1:model.nconpath + i = 35; + + q1= X((model.nx+model.nu)*(i-1)+2); + q2= X((model.nx+model.nu)*(i-1)+3); + q3= X((model.nx+model.nu)*(i-1)+4); + q4= X((model.nx+model.nu)*(i-1)+5); + q5= X((model.nx+model.nu)*(i-1)+6); + + % wristXmine(i) = par.Yankle + par.ShankLen*sin(pi-q1(i)) -par.ThighLen*sin(q2(i)-pi+q1(i))+... + % par.TrunkLen*sin(-q2(i)-q1(i)-q3(i)+2*pi)- par.UpparmLen*sin(-q2(i)-q1(i)-q3(i)-q4(i)+2*pi)+... + % par.ForearmLen*sin(-2*pi+q2(i)+q1(i)+q3(i)+q4(i)+q5(i)); + % c(model.ncondyn + model.ncontaskper + model.ncontaskcbl+2) = (par.Yankle + par.ShankLen*sin(pi-q1) -... + % par.ThighLen*sin(q2-pi+q1)+... + % par.TrunkLen*sin(-q2-q1-q3+2*pi)- par.UpparmLen*sin(-q2-q1-q3-q4+2*pi)+... + % par.ForearmLen*sin(-2*pi+q2+q1+q3+q4+q5))*model.cgain; + + J(model.ncondyn + model.ncontaskper + model.ncontaskcbl+1 , (model.nx+model.nu)*(i-1) +2 ) = (-par.ShankLen*cos(pi-q1)-... + par.ThighLen*cos(q2+q1-pi)-... + par.TrunkLen*cos(-q2-q1-q3)+... + par.UpparmLen*cos(-q2-q1-q3-q4) +... + par.ForearmLen*cos(q2+q1+q3+q4+q5))*model.cgain; + + J(model.ncondyn + model.ncontaskper + model.ncontaskcbl+1 , (model.nx+model.nu)*(i-1) +3 ) = (-par.ThighLen*cos(q2+q1-pi)-... + par.TrunkLen*cos(-q2-q1-q3)+ ... + par.UpparmLen*cos(-q2-q1-q3-q4)+... + par.ForearmLen*cos(q2+q1+q3+q4+q5))*model.cgain; + + J(model.ncondyn + model.ncontaskper + model.ncontaskcbl+1 , (model.nx+model.nu)*(i-1) +4 ) = (-par.TrunkLen*cos(-q2-q1-q3)+... + par.UpparmLen*cos(-q2-q1-q3-q4)+... + par.ForearmLen*cos(q2+q1+q3+q4+q5))*model.cgain; + + J(model.ncondyn + model.ncontaskper + model.ncontaskcbl+1, (model.nx+model.nu)*(i-1) +5 ) = (par.UpparmLen*cos(-q2-q1-q3-q4)+... + par.ForearmLen*cos(q2+q1+q3+q4+q5))*model.cgain; + + J(model.ncondyn + model.ncontaskper + model.ncontaskcbl+1, (model.nx+model.nu)*(i-1) +6 ) = (par.ForearmLen*cos(q2+q1+q3+q4+q5))*model.cgain; + + if model.nconpath==2 + j = 36; + + q1= X((model.nx+model.nu)*(j-1)+2); + q2= X((model.nx+model.nu)*(j-1)+3); + q3= X((model.nx+model.nu)*(j-1)+4); + q4= X((model.nx+model.nu)*(j-1)+5); + q5= X((model.nx+model.nu)*(j-1)+6); + + % wristXmine(i) = par.Yankle + par.ShankLen*sin(pi-q1(i)) -par.ThighLen*sin(q2(i)-pi+q1(i))+... + % par.TrunkLen*sin(-q2(i)-q1(i)-q3(i)+2*pi)- par.UpparmLen*sin(-q2(i)-q1(i)-q3(i)-q4(i)+2*pi)+... + % par.ForearmLen*sin(-2*pi+q2(i)+q1(i)+q3(i)+q4(i)+q5(i)); + % model.ncondyn + model.ncontaskper + model.ncontaskcbl +( 1:i) + J(model.ncondyn + model.ncontaskper + model.ncontaskcbl+2 , (model.nx+model.nu)*(j-1) +2 ) = (-par.ShankLen*cos(pi-q1)-... + par.ThighLen*cos(q2+q1-pi)-... + par.TrunkLen*cos(-q2-q1-q3)+... + par.UpparmLen*cos(-q2-q1-q3-q4) +... + par.ForearmLen*cos(q2+q1+q3+q4+q5))*model.cgain; + + J(model.ncondyn + model.ncontaskper + model.ncontaskcbl+2 , (model.nx+model.nu)*(j-1) +3 ) = (-par.ThighLen*cos(q2+q1-pi)-... + par.TrunkLen*cos(-q2-q1-q3)+ par.UpparmLen*cos(-q2-q1-q3-q4)+... + par.ForearmLen*cos(q2+q1+q3+q4+q5))*model.cgain; + + J(model.ncondyn + model.ncontaskper + model.ncontaskcbl+2 , (model.nx+model.nu)*(j-1) +4 ) = (-par.TrunkLen*cos(-q2-q1-q3)+... + par.UpparmLen*cos(-q2-q1-q3-q4)+... + par.ForearmLen*cos(q2+q1+q3+q4+q5))*model.cgain; + + J(model.ncondyn + model.ncontaskper + model.ncontaskcbl+2, (model.nx+model.nu)*(j-1) +5 ) = (par.UpparmLen*cos(-q2-q1-q3-q4)+... + par.ForearmLen*cos(q2+q1+q3+q4+q5))*model.cgain; + + J(model.ncondyn + model.ncontaskper + model.ncontaskcbl+2, (model.nx+model.nu)*(j-1) +6 ) = (par.ForearmLen*cos(q2+q1+q3+q4+q5))*model.cgain; + + end + end + end + if isfield(model,'Reqpower') + + ivel = 7:model.nx+model.nu:(model.N-2)*(model.nx+model.nu)+7; + iF = 13:model.nx+model.nu:(model.N-2)*(model.nx+model.nu)+13; + V_fw = X(ivel); + F = X(iF); +% c(model.ncondyn + model.ncontaskper + model.ncontaskcbl+model.nconpath+1) = sum(V_fw .* F) /(model.N) - model.Reqpower; + + for i=1:model.N-1 + + J(model.ncondyn + model.ncontaskper + model.ncontaskcbl+model.nconpath+1, ivel(i)) = F(i)/(model.N-1); + J(model.ncondyn + model.ncontaskper + model.ncontaskcbl+model.nconpath+1, iF(i)) = V_fw(i)/(model.N-1); + end -% J(end , end-model.npar+1) = f* dLi1dt + (1-f)*dLi2dt; - J(end , (model.nx+model.nu)*(i-1) + (2:6)) = f* dLi1dt ; - J(end , (model.nx+model.nu)*i + (2:6)) = (1-f)*dLi2dt; - - dLtmaxdf = Li1 - Li2; - dfdtmax = 1/(model.time(i+1)-model.time(i)); - J(end, end) = dLtmaxdf*dfdtmax; - - + end end %==================================================================== @@ -516,5 +910,4 @@ function matcompare(a,b) irow = irow(icol); fprintf('%9.6f at %d %d (%9.6f vs. %9.6f)\n', full(maxerr), irow, icol, full(a(irow,icol)), full(b(irow,icol))); end - - + \ No newline at end of file diff --git a/rowingtest.mat b/rowingtest.mat index 4eb819f..bbcb8d5 100644 Binary files a/rowingtest.mat and b/rowingtest.mat differ diff --git a/tmp.avi b/tmp.avi index f78c4e2..7f08f8a 100644 Binary files a/tmp.avi and b/tmp.avi differ diff --git a/tools/bodyangles.m b/tools/bodyangles.m index 91f7cb4..c4ce5e2 100644 --- a/tools/bodyangles.m +++ b/tools/bodyangles.m @@ -1,8 +1,9 @@ function [q,t2,p] = bodyangles % better fucntion name the part that produce p and q + global problem % load X2016_03_17_trial2_markers - load Mdata2 + load('../data/Mdata2'); %choose the averaged trial number (to underestand how the data is averaged visit "how to make data note") d = MDATA; @@ -69,43 +70,61 @@ legend('q1','q2','q3','q4','q5'); set(gcf,'renderer','painters'); - % determine model parameters - p.ShankLen = mean(sqrt((kneeY-ankleY).^2+(kneeZ-ankleZ).^2)); - p.ThighLen = mean(sqrt((hipY-kneeY).^2+(hipZ-kneeZ).^2)); - p.TrunkLen = mean(sqrt((shoulderY-hipY).^2+(shoulderZ-hipZ).^2)); - p.UpparmLen = mean(sqrt((elbowY-shoulderY).^2+(elbowZ-shoulderZ).^2)); - p.ForearmLen = mean(sqrt((wristY-elbowY).^2+(wristZ-elbowZ).^2)); - % calculate the coordinate of ankle point, and assume it is fixed p.Xankle = mean(ankleZ); p.Yankle = mean(ankleY); - + % do the linear polyfit of hip motion track for the alpha angle coeff = polyfit(hipZ,hipY,1); p.a=coeff(1,1); p.b=coeff(1,2); %FIT A STAIGHT LINE THROUGH A LINEAR REGRETION - % define scale factor to convert kg to Mg and N to kN - scale = 0.001; - p.M = scale * 80; % body mass (kg) - % determins sprocket position based on the most forward position of the % wrist (IN FUTURE WORK WE NEED A MARKER) %i = find(max(wristZ)); [p.Xsprocket,i] = max(wristZ); p.Xsprocket = max(wristZ) + 0.1; p.Ysprocket = wristY(i); - - % These are model parameters + + % determine model parameters + p.ShankLen = mean(sqrt((kneeY-ankleY).^2+(kneeZ-ankleZ).^2)); + p.ThighLen = mean(sqrt((hipY-kneeY).^2+(hipZ-kneeZ).^2)); + p.TrunkLen = mean(sqrt((shoulderY-hipY).^2+(shoulderZ-hipZ).^2)); + p.UpparmLen = mean(sqrt((elbowY-shoulderY).^2+(elbowZ-shoulderZ).^2)); + p.ForearmLen = mean(sqrt((wristY-elbowY).^2+(wristZ-elbowZ).^2)); + + % define scale factor to convert kg to Mg and N to kN + scale = 0.001; + p.M = scale * 80; % body mass (kg) + + % These are model parameters p.g = 9.80665; % gravity(N/kg) - p.Kseat = scale * 10^5; % seat stiffness (kN/m) - p.Cseat = 2*sqrt(p.M*p.Kseat); % seat damping (kNs/m), close to critical damping + p.Kseat = 10^5; % seat stiffness (kN/m) + p.Cseat = 2*sqrt(p.M*p.Kseat); % seat damping (kNs/m), close to critical damping % rowing machine parameters from parameter identification - p.C = scale * 89.68; % flywheel damping coefficient kN/(m/s)^2 - p.m = scale * 539.6; % flywheel equivalent mass - p.Kcrm = scale * 28386; % stiffness of cable and ratchet mechanism (kN/m) (CHECK VALUE IN PARAMETER IDENTIFICATION - p.K = scale * 12.97 ; % shock cord stiffness (kN/m) + p.C = scale * 92.7427; % flywheel damping coefficient kN/(m/s)^2 + p.m = scale * 557.6094; % flywheel equivalent mass + if strcmp(problem.component,'spring') + p.Kcrm = scale * 28386; % stiffness of cable and ratchet mechanism (kN/m) (CHECK VALUE IN PARAMETER IDENTIFICATION + p.Bcrm = scale * 0; + elseif strcmp(problem.component,'damper') + p.Bcrm = scale * 3111.8023; % damping of ratchet mechanism (kNs/m) + p.Kcrm = scale * 0; + else + warning('Please especify the proper component (spring/damper)'); + end + +% Estimated model parameters: +% b1 = 0.0000 Ns/m +% c1 = 92.7427 Ns^2/m^2 +% m1 = 557.6094 kg +% k = 0.0000 N/m +% b2 = 3111.8023 Ns/m +% m2 = 0.0000 kg +% k2 = 14.0700 N/m +% epsilon = 0.0001 + p.K = scale * 14.07 ; % shock cord stiffness (kN/m) p.L0 = 0.2; % the wrist-sprocket distance at which shock cord has no force % body segment parameters calculated using the tables from Winter's book @@ -123,7 +142,8 @@ p.ThighCM = (1-0.433)*p.ThighLen; p.TrunkCM = (1-0.500)*p.TrunkLen; p.UpparmCM = 0.436*p.UpparmLen; - p.ForearmCM = 0.430*p.ForearmLen; + p.ForearmCM = 0.430*p.ForearmLen; + %q = [q1,q2,q3,q4,q5]; % extract joint angle trajectories (states 1-5) %figure(9); %animate(q,p); diff --git a/tools/datainterp2.m b/tools/datainterp2.m index 6706d5b..ce28edd 100644 --- a/tools/datainterp2.m +++ b/tools/datainterp2.m @@ -1,9 +1,7 @@ - function [dSPACEdata, qdata, t, p] = datainterp2 global model - load('Mdata2.mat'); - load('data2richter.mat'); + load('../data/data2richter.mat'); model.r = 0.0135; HMCdata.tvect = DATA(:,1); %time @@ -16,15 +14,11 @@ % time grid with N points HMCdata.tvect = HMCdata.tvect - HMCdata.tvect(1); % ensure that time starts at zero HMCdata.T = HMCdata.tvect(end); % duration of the rowing cycle - %HMCdata.force = HMCdata.force/1000; % we do everything in kN [qdata,t2,p] = bodyangles; % One rowing cycle frames = 1087:1278; % One rowing cycle (included both pulling and returning phases) - %frames = 1087:1187; % half a rowing cycle (the first phase) - %frames = 1188:1287; % half a rowing cycle (the second phase) - t2 = t2(frames); qdata = qdata(frames,:);