3B

% 3B
clear all,
clc
XYZ1=zeros(10,3);
XYZ2=zeros(10,3);
XYZ3=zeros(10,3);
XYZ4=zeros(10,3);
XYZ5=zeros(10,3);
syms d1 a1 th2 a3 q2 th4 a4 al5 q4 q5 al3 al4
% determination of a symbolic form of HT matrices –
% application of mA function
A1=mA(0,d1,a1,0);
A2=mA(th2,0,0,0);
A3=mA(0,0,a3,al3);
A4=mA(th4,0,a4,al4);
A5=mA(0,0,0,al5);
% multiplication of matrices
T01=A1;
T02=A1*A2;
T03=A1*A2*A3;
T04=A1*A2*A3*A4;
T05=A1*A2*A3*A4*A5;

D1=[430 350 200 600 100 800 170 320 560 730];
TH2=(pi/180).*[20 -45 -70 0 65 -90 80 30 -50 60];
A3=[250 400 100 600 530 170 375 320 220 190];
TH4=(pi/180).*[-10 -40 -110 50 75 -120 0 -35 80 -60];
AL5=(pi/180).*[0 60 120 -45 -80 -35 70 25 -30 55];

    for i=1:1:10

        for j = 1:1:10

    if D1(1,i)==100
        D11=zeros(1,10);
        D11(1:10)=100;
    else
        D11=(100:(D1(1,i)-100)/9:D1(1,i));
    end

    if TH2(1,i)==0
        TH22=zeros(1,10);
    else
        TH22=(0:TH2(1,i)/9:TH2(1,i));
    end

    if A3(1,i)==100
        A33=zeros(1,10);
        A33(1:10)=100;
    else
        A33=(100:(A3(1,i)-100)/9:A3(1,i));
    end

    if TH4(1,i)==0
        TH44=zeros(1,10);
    else
        TH44=(0:TH4(1,i)/9:TH4(1,i));
    end

    if AL5(1,i)==0
        AL55=zeros(1,10);
    else
        AL55=(0:AL5(1,i)/9:AL5(1,i));
    end

T01n=double(subs(T01,{d1,a1},{D11(1,j),500}));
T02n=double(subs(T02,{d1,a1,th2},{D11(1,j),500,TH22(1,j)}));
T03n=double(subs(T03,{d1,a1,th2,a3,al3},{D11(1,j),500,TH22(1,j),A33(1,j),-pi/2}));
T04n=double(subs(T04,{d1,a1,th2,a3,al3,th4,a4,al4},{D11(1,j),500,TH22(1,j),A33(1,j),-pi/2,TH44(1,j),500,pi/2}));
T05n=double(subs(T05,{d1,a1,th2,a3,al3,th4,a4,al4,al5},{D11(1,j),500,TH22(1,j),A33(1,j),-pi/2,TH44(1,j),500,pi/2,AL55(1,j)}));

XYZ1(j,1,i) = T01n(1,4);
XYZ1(j,2,i) = T01n(2,4);
XYZ1(j,3,i) = T01n(3,4);

XYZ2(j,1,i) = T02n(1,4);
XYZ2(j,2,i) = T02n(2,4);
XYZ2(j,3,i) = T02n(3,4);

XYZ3(j,1,i) = T03n(1,4);
XYZ3(j,2,i) = T03n(2,4);
XYZ3(j,3,i) = T03n(3,4);

XYZ4(j,1,i) = T04n(1,4);
XYZ4(j,2,i) = T04n(2,4);
XYZ4(j,3,i) = T04n(3,4);

XYZ5(j,1,i) = T05n(1,4);
XYZ5(j,2,i) = T05n(2,4);
XYZ5(j,3,i) = T05n(3,4);
        end
    end



        for k=1:10
figure(k)


plot3([0 XYZ1(10,1,k)],[0 XYZ1(10,2,k)],[0 XYZ1(10,3,k)])
hold on
plot3([XYZ1(10,1,k) XYZ2(10,1,k)],[XYZ1(10,2,k) XYZ2(10,2,k)],[XYZ1(10,3,k) XYZ2(10,3,k)])
hold on
plot3([XYZ2(10,1,k) XYZ3(10,1,k)],[XYZ2(10,2,k) XYZ3(10,2,k)],[XYZ2(10,3,k) XYZ3(10,3,k)])
hold on
plot3([XYZ3(10,1,k) XYZ4(10,1,k)],[XYZ3(10,2,k) XYZ4(10,2,k)],[XYZ3(10,3,k) XYZ4(10,3,k)])
hold on
plot3([XYZ4(10,1,k) XYZ5(10,1,k)],[XYZ4(10,2,k) XYZ5(10,2,k)],[XYZ4(10,3,k) XYZ5(10,3,k)])
hold on

plot3(XYZ1(10,1,k),XYZ1(10,2,k),XYZ1(10,3,k),'-D')
hold on
plot3(XYZ2(10,1,k),XYZ2(10,2,k),XYZ2(10,3,k),'-O')
hold on
plot3(XYZ3(10,1,k),XYZ3(10,2,k),XYZ3(10,3,k),'-D')
hold on
plot3(XYZ4(10,1,k),XYZ4(10,2,k),XYZ4(10,3,k),'-O')
hold on
plot3(XYZ5(10,1,k),XYZ5(10,2,k),XYZ5(10,3,k),'-X')

axis([-2000 2000 -2000 2000 -2000 2000])

plot3(XYZ5(:,1,k),XYZ5(:,2,k),XYZ5(:,3,k),'--r')
xlabel('X')
ylabel('Y')
zlabel('Z')
title (sprintf('Path number %d of end effector',k))
        end

for m=1:10
    figure (m+10)
plot3(XYZ5(:,1,m),XYZ5(:,2,m),XYZ5(:,3,m),'-r')
xlabel('X')
ylabel('Y')
zlabel('Z')
title (sprintf('Path number %d of end effector',m))
end