Full code:
height= 1.85; %[m]
mass= 85; %[kg]
tighR=0.08; %[m]
legR= 0.05; %[m]
% x
gaitCycleDuration= 1.4; %[second]
x= linspace(0,1.0,95);
xx= linspace(0,1.00,101);
%HİP
%hip angle
hip_angle= [0.4136 0.4128 0.4121 0.4100 0.4047 0.3958 0.3831 0.3672 0.3485 0.3274 0.3047 0.2805 0.2553 0.2295 0.2030 0.1758 0.1484 0.1208 0.0932 0.0661 0.0396 0.0138 -0.0112 -0.0354 -0.0586 -0.0808 -0.1019 -0.1218 -0.1401 -0.1571 -0.1724 -0.1862 -0.1983 -0.2082 -0.2161 -0.2217 -0.2248 -0.2251 -0.2224 -0.2168 -0.2087 -0.1990 -0.1887 -0.1791 -0.1712 -0.1658 -0.1635 -0.1641 -0.1670 -0.1716 -0.1766 -0.1813 -0.1861 -0.1909 -0.1967 -0.2032 -0.2100 -0.2157 -0.2197 -0.2211 -0.2194 -0.2145 -0.2066 -0.1962 -0.1834 -0.1684 -0.1517 -0.1335 -0.1138 -0.0934 -0.0721 -0.0478 -0.0241 0.0002 0.0250 0.0497 0.0745 0.0990 0.1229 0.1461 0.1686 0.1901 0.2108 0.2306 0.2492 0.2667 0.2829 0.2978 0.3117 0.3245 0.3362 0.3471 0.3571 0.3665 0.3752 0.3831 0.3906 0.3972 0.4030 0.4077 0.4112];
P = polyfit(xx,hip_angle,6);
Y = polyval(P,xx);
hip_angle=[Y(3) Y(4:95) Y(3) Y(3)];
figure
grid on;
hold on
plot(x,hip_angle,'k');
xlabel('Gait Cycle (%)')
ylabel('Hip Angle (rad)')
hold off
%hip velocity
P_prime = polyder(P);
hip_velocity = polyval(P_prime,xx);
hip_velocity= [0 hip_velocity(4:96) 0];
figure;
grid on;
hold on
plot(x,hip_velocity,'b');
xlabel('Gait Cycle (%)')
ylabel('Hip Veloicty (rad/s)')
hold off
%hip accelaration
P_primeprime = polyder(P_prime);
hip_acceleration = polyval(P_primeprime,xx);
hip_acceleration= [hip_acceleration(3) hip_acceleration(4:96) hip_acceleration(3)];
figure;
grid on;
hold on
plot(x,hip_acceleration,'b');
xlabel('Gait Cycle (%)')
ylabel('Hip Accelartion (rad/s^2)')
hold off
xxx=linspace(0,1,95);
xxxx=linspace(0,1.02,98);
%KNEE
%knee angle
knee_angle = [0.141575 0.173390 0.208110 0.246116 0.286897 0.329180 0.370968 0.410155 0.445131 0.474860 0.498782 0.516788 0.529155 0.536236 0.538367 0.535901 0.529167 0.518466 0.504447 0.487239 0.466171 0.443262 0.418379 0.392150 0.365140 0.337819 0.310595 0.283848 0.257954 0.233173 0.209699 0.187784 0.167653 0.149496 0.133475 0.119758 0.108498 0.099792 0.093761 0.090577 0.090379 0.093314 0.099529 0.109094 0.122028 0.138319 0.157942 0.180881 0.207177 0.237052 0.251327 0.270526 0.304515 0.345870 0.392038 0.442991 0.498452 0.557778 0.620227 0.685252 0.752074 0.819330 0.885340 0.948415 1.007006 1.059808 1.105818 1.144343 1.174976 1.197558 1.212168 1.219045 1.218579 1.211167 1.197207 1.177086 1.151122 1.119633 1.082898 1.041178 0.994764 0.943959 0.889051 0.830301 0.767978 0.702337 0.633677 0.562432 0.489219 0.415088 0.341775 0.271665 0.207522 0.152104 0.107945 0.076989 0.060235 0.057484 0.067176 0.086535 0.112247];
P = polyfit(xx,knee_angle,6);
Y = polyval(P,xxx);
knee_angle=[Y(2:94) Y(1) Y(2)];
figure
grid on;
hold on
plot(xxx,knee_angle)
xlabel('Gait Cycle (%)')
ylabel('Knee Angle (rad)')
hold off
%knee velocity
P_prime = polyder(P);
knee_velocity = polyval(P_prime,xxxx);
knee_velocity= [knee_velocity(2:94) knee_velocity(1) knee_velocity(2)];
figure;
grid on;
hold on
plot(x,knee_velocity,'b');
xlabel('Gait Cycle (%)')
ylabel('Knee Veloicty (rad/s)')
hold off
%knee accelartion
P_primeprime = polyder(P_prime);
knee_acceleration = polyval(P_primeprime,xxxx);
knee_acceleration= [knee_acceleration(2:91) 149 140 100 knee_acceleration(1)+5 knee_acceleration(2)];
figure;
grid on;
hold on
plot(x,knee_acceleration,'b');
xlabel('Gait Cycle (%)')
ylabel('Knee Accelartion (rad/s^2)')
hold off
link1maxJ=(100/178)*42/100*height; %length of link 1 (thigh + joint length) [m]
link2maxJ=(100/178)*52/100*height; %length of link 2 (joint + leg length) [m]
%To find max and min link lengths for SW, irrelevant with the code
link1min=(100/178)*40/100*1.55;
link2min=(100/178)*45/100*1.55;
link1max=(100/178)*40/100*height;
link2max= (100/178)*45/100*height;
%PARAMETERS
normal_force = [27.838 55.676 83.514 111.352 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 139.19 111.352 83.514 55.676 27.838 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0];
friction_force= [13.919 27.838 41.757 55.676 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 69.595 55.676 41.757 27.838 13.919 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0];
% m11= 0.167*mass; %mass of total leg [kg]
ml1= 1.877; %mass of the thig link [kg]
ml2= 2.049; %mass of the shank link [kg]
mM1=0.5; %mass of the hip motor [kg]
mM2=0.5; %mass of the knee motor [kg]
m1= 0.105*mass + ml1 + mM1; %mass of thigh leg [kg]
m2= 0.0475*mass + ml2 + mM2; %mass of leg + foot [kg]
d1=link1max/2; % link1/2 [m]
d2=link2max/2; % link2/2 [m]
%MOMENT OF INERTIA
I1=1/3*m1*tighR^2;
I2=1/3*m2*legR^2; %rod moment of inertia [kg/m^2]
n=0;
torque1= [];
torque2= [];
force1= [];
force2= [];
force3= [];
force4= [];
for n= 0: length(hip_angle)-1
n= n+1;
q1= hip_angle(n);
q1d= hip_velocity(n);
q1dd= hip_acceleration(n);
q2= knee_angle(n);
q2d= knee_velocity(n);
q2dd= knee_acceleration(n);
f5= friction_force(n);
f6= normal_force(n);
syms T1 T2 f1 f2 f3 f4 ;
E1=m1*((-d1.*sin(q1).*q1d.^2)+(d1.*cos(q1).*q1dd))-(f2-f4);
E2=m1*((-d1.*cos(q1).*q1d.^2)-(d1.*sin(q1).*q1dd))-(f3-f1);
E3=T1+(f1*d1.*sin(q1))-(f2.*d1.*cos(q1))+(f3.*d1.*sin(q1))-(f4.*d1.*cos(q1))-(q1dd.*I1);
E4=m2*((-link1max.*sin(q1).*q1d.^2)+(link1max.*cos(q1).*q1dd)-(d2.*sin(q2).*q2d.^2)+(d2.*cos(q2).*q2dd))-(f4-f5);
E5=m2*((-link1max.*cos(q1).*q1d.^2)-(link1max.*sin(q1).*q1dd)-(d2.*cos(q2).*q2d.^2)-(d2.*sin(q2).*q2dd))-(f6-f3);
E6=T2+(f6*d2.*sin(q2))-(f5.*d2.*cos(q2))+(f3.*d2.*sin(q2))-(f4*d2.*cos(q2))-(q2dd.*I2);
[T1,T2,f1,f2,f3,f4]= solve(E3,E6,E2,E4,E5,E1);
torque1(end+1)=eval(T1); %Nm
torque2(end+1)=eval(T2);
force1(end+1)=eval(f1);
force2(end+1)=eval(f2);
force3(end+1)=eval(f3);
force4(end+1)=eval(f4);
end