function taus = calcTauFourLinkBody(x)

% Define constants e.g. 
global M g l k
M = 65;
g = 9.81;

Mb=0.04*M;
Mc=0.12*M;
Md=0.12*M;
Me=0.04*M;
Mf=0.68*M;
lb=0.25;
lc=0.25;
ld=0.25;
le=0.25;
lf=0.5;
%taub=0;
%tauc=0;
%taud=0;
%taue=0;
%tauf=0;

% Define forces: 

% State assignments
q1 = x(1); q2 = x(2); q3 = x(3); q4 = x(4); q5 = x(5); 
u1 = x(6); u2 = x(7); u3 = x(8); u4 = x(9); u5 = x(10); 
u1dot=x(11); u2dot=x(12); u3dot=x(13); u4dot=x(14); u5dot=x(15);
s1 = sin(q1); s2 = sin(q2); s3 = sin(q3); s4 = sin(q4); s5 = sin(q5); c1m2 = cos(q1 - q2); c1m3 = cos(q1 - q3); c1m4 = cos(q1 - q4); c1m5 = cos(q1 - q5); c2m3 = cos(q2 - q3); c2m4 = cos(q2 - q4); c2m5 = cos(q2 - q5); c4m5 = cos(q4 - q5); s1m2 = sin(q1 - q2); s1m3 = sin(q1 - q3); s1m4 = sin(q1 - q4); s1m5 = sin(q1 - q5); s2m3 = sin(q2 - q3); s2m4 = sin(q2 - q4); s2m5 = sin(q2 - q5); s4m5 = sin(q4 - q5); 

MM = zeros(5,5); rhs_noTau = zeros(5,1); udot = zeros(5,1);
udot=[u1dot;u2dot;u3dot;u4dot;u5dot];

% Mass Matrix
MM(1,1) = 0.05 + Mb*(lb*lb) + 4*Mc*(lb*lb) + 4*Md*(lb*lb) + 4*Me*(lb*lb) + 4*Mf*(lb*lb); 
MM(1,2) = 2*c1m2*lb*lc*Mc + 4*c1m2*lb*lc*Md + 4*c1m2*lb*lc*Me + 4*c1m2*lb*lc*Mf; 
MM(1,3) = 2*c1m3*lb*lf*Mf; 
MM(1,4) = 2*c1m4*lb*ld*Md + 4*c1m4*lb*ld*Me; 
MM(1,5) = 2*c1m5*lb*le*Me; 
MM(2,1) = MM(1,2); 
MM(2,2) = 0.05 + Mc*(lc*lc) + 4*Md*(lc*lc) + 4*Me*(lc*lc) + 4*Mf*(lc*lc); 
MM(2,3) = 2*c2m3*lc*lf*Mf; 
MM(2,4) = 2*c2m4*lc*ld*Md + 4*c2m4*lc*ld*Me; 
MM(2,5) = 2*c2m5*lc*le*Me; 
MM(3,1) = MM(1,3); 
MM(3,2) = MM(2,3); 
MM(3,3) = 0.05 + Mf*(lf*lf); 
MM(3,4) = 0; 
MM(3,5) = 0; 
MM(4,1) = MM(1,4); 
MM(4,2) = MM(2,4); 
MM(4,3) = MM(3,4); 
MM(4,4) = 0.05 + Md*(ld*ld) + 4*Me*(ld*ld); 
MM(4,5) = 2*c4m5*ld*le*Me; 
MM(5,1) = MM(1,5); 
MM(5,2) = MM(2,5); 
MM(5,3) = MM(3,5); 
MM(5,4) = MM(4,5); 
MM(5,5) = 0.05 + Me*(le*le); 

% righthand side terms minus torques
rhs_noTau(1) = s1*g*lb*Mb + 2*s1*g*lb*Mc + 2*s1*g*lb*Md + 2*s1*g*lb*Me + ...
2*s1*g*lb*Mf + s1m2*(-2*lb*lc*Mc - 4*lb*lc*Md - 4*lb*lc*Me - ...
4*lb*lc*Mf)*(u2*u2) - 2*s1m3*lb*lf*Mf*(u3*u3) + s1m4*(-2*lb*ld*Md - ...
4*lb*ld*Me)*(u4*u4) - 2*s1m5*lb*le*Me*(u5*u5); 

rhs_noTau(2) = s2*g*lc*Mc + 2*s2*g*lc*Md + 2*s2*g*lc*Me + 2*s2*g*lc*Mf + ...
s1m2*(2*lb*lc*Mc + 4*lb*lc*Md + 4*lb*lc*Me + 4*lb*lc*Mf)*(u1*u1) - ...
2*s2m3*lc*lf*Mf*(u3*u3) + s2m4*(-2*lc*ld*Md - 4*lc*ld*Me)*(u4*u4) - ...
2*s2m5*lc*le*Me*(u5*u5); 

rhs_noTau(3) = s3*g*lf*Mf + 2*s1m3*lb*lf*Mf*(u1*u1) + ...
2*s2m3*lc*lf*Mf*(u2*u2); 

rhs_noTau(4) = s4*g*ld*Md + 2*s4*g*ld*Me + s1m4*(2*lb*ld*Md + ...
4*lb*ld*Me)*(u1*u1) + s2m4*(2*lc*ld*Md + 4*lc*ld*Me)*(u2*u2) - ...
2*s4m5*ld*le*Me*(u5*u5); 

rhs_noTau(5) = s5*g*le*Me + 2*s1m5*lb*le*Me*(u1*u1) + ...
2*s2m5*lc*le*Me*(u2*u2) + 2*s4m5*ld*le*Me*(u4*u4); 

%udot = MM\rhs;
%qdot = zeros(5,1);
%MM*udot=taus+rhs_noTau;

taus=MM*udot-rhs_noTau
%Kinematic Constraints
%qdot(1) = u1; 
%qdot(2) = u2;
%qdot(3) = u3;
%qdot(4) = u4;
%qdot(5) = u5;

%xdot = [qdot; udot];