SimParam


Data=load('DataAuto.txt');
q1=Data(:,1);
q2=Data(:,2);
q3=Data(:,3);
q4=Data(:,4);
q5=Data(:,5);
q6=Data(:,6);

t=linspace(0,4,290);

figure (1)
plot(t,q1,'LineWidth',2)
hold on
plot(t,q2,'r','LineWidth',2)
hold on
plot(t,q3,'g','LineWidth',2)
hold on
plot(t,q4,'k','LineWidth',2)
hold on
plot(t,q5,'y','LineWidth',2)
hold on
plot(t,q6,'m','LineWidth',2)
hold on
xlabel('time(s)','FontSize',14)
ylabel('Joint Angles','FontSize',14)
legend('Ankle joint (lateral plane)','Ankle joint (sagittal plane)','Knee joint','Hip joint (vertical axis)','Hip joint (lateral plane)','Hip joint (sagittal plane)')


for i=1:max(size(q1))
P_ref_r = [0 0 0]; 
qi_r = [q1(i) q2(i) q3(i) q4(i) q5(i) q6(i)]';   qi_l = [0 0 0 0 0 0]'; 
[P_r(:,i),P_hip] = DirKin6dof_stance( P_ref_r,qi_r,L,+1 );

end

for i=1:max(size(P_r))

P1(1,i)=P_r(1,i);
P1(2,i)=P_r(2,i);
P1(3,i)=P_r(3,i);


P2(1,i)=P_r(4,i);
P2(2,i)=P_r(5,i);
P2(3,i)=P_r(6,i);


P3(1,i)=P_r(7,i);
P3(2,i)=P_r(8,i);
P3(3,i)=P_r(9,i);


P4(1,i)=P_r(10,i);
P4(2,i)=P_r(11,i);
P4(3,i)=P_r(12,i);


P5(1,i)=P_r(13,i);
P5(2,i)=P_r(14,i);
P5(3,i)=P_r(15,i);
end

t=linspace(0,4.5,max(size(P1)))
figure (2)
plot(t,P1(2,:))
hold on
plot(t,P1(3,:),'r');
hold on
plot(t,P1(1,:),'g');
legend('X','Y','Z')
title('Stance foot','FontSize',14)
hold off

figure (3)
plot(t,P2(2,:))
hold on
plot(t,P2(3,:),'r');
hold on
plot(t,P2(1,:),'g');
legend('X','Y','Z')
title('Stance ankle','FontSize',14)
hold off
%figure (3)
%plot3(P2(1,:),P2(2,:),P2(3,:))

figure (4)
plot(t,P3(2,:))
hold on
plot(t,P3(3,:),'r');
hold on
plot(t,P3(1,:),'g');
legend('X','Y','Z')
title('Stance knee','FontSize',14)
hold off
%figure (4)
%plot3(P3(1,:),P3(2,:),P3(3,:))
figure (5)
plot(t,P4(2,:))
hold on
plot(t,P4(3,:),'r');
hold on
plot(t,P4(1,:),'g');
legend('X','Y','Z')
title('Stance vertical axis','FontSize',14)
hold off
%figure (5)
%plot3(P4(1,:),P4(2,:),P4(3,:))
figure (6)
plot(t,P5(2,:))
hold on
plot(t,P5(3,:),'r');
hold on
plot(t,P5(1,:),'g');
legend('X','Y','Z')
title('Stance hip','FontSize',14)
hold off

