% this script determines a Homogeneous Transformation HT matrix
clear all
% declaration of symbols
syms d1 a1 a2 th2 d3 q1 q2 q3 th4 a4 a5 al1 al4 q4 q5 th5 a3
% determination of a symbolic form of HT matrices –
% application of mA function
A1=mA(0,d1,a1,al1)
A2=mA(th2,0,a2,0)
A3=mA(0,d3,0,0)
A4=mA(th4,0,a4,al4)
A5=mA(th5,0,a5,0)
% multiplication of matrices
T01=A1;
T02=A1*A2;
T03=A1*A2*A3
T04=A1*A2*A3*A4;
T35=A4*A5
T05=A1*A2*A3*A4*A5;
% substitution of rotational joint variables
% for the simplification purpose
T01v=subs(T02,{al1},{q1});
T02v=subs(T02,{th2},{q2});
T03v=subs(T03,{al1,th2},{q1,q2});
T04v=subs(T04,{al1,th2,th4,al4},{q1,q2,q3,q4})
T35v=subs(T35,{th4,th5},{q4,q5});
T05v=subs(T05,{al1,th2,th4,al4,th5},{q1,q2,q3,q4,q5});
% indication of joint coordinates
% variables: d1,th2,a3,th4 and al5 indicated by ‘1’s
zmie1=[[0,1,0,0]]
zmie2=[[0,1,0,0];[1,0,0,0]]
zmie3=[[0,1,0,0];[1,0,0,0];[0,1,0,0]]
zmie4=[[0,1,0,0];[1,0,0,0];[0,1,0,0];[1,0,0,0]]
zmie5=[[0,1,0,0];[1,0,0,0];[0,1,0,0];[1,0,0,0];[1,0,0,0]]
zmie6=[[1,0,0,0];[1,0,0,0]]
% a simplified form of the evaluated HT matrices
% for interpretation purpose for a user
T01u=zam(zmie1,T01v,'q')
T02u=zam(zmie2,T02v,'q')
T03u=zam(zmie3,T03v,'q')
T04u=zam(zmie4,T04v,'q')
T05u=zam(zmie5,T05v,'q')
T35u=zam(zmie6,T35v,'q')
% numerical matrices
T01n=double(subs(T01,{d1,a1,al1},{500,200,pi/2}))
T02n=double(subs(T02,{d1,a1,al1,th2,a2},{500,200,pi/2,0,500}))
T03n=double(subs(T03,{d1,a1,al1,th2,a2,d3},{500,200,pi/2,0,500,500}))
T04n=double(subs(T04,{d1,a1,al1,th2,a2,d3,th4,a4,al4},{500,200,pi/2,0,500,500,0,500,-pi/2}))
T05n=double(subs(T05,{d1,a1,al1,th2,a2,d3,th4,a4,al4,th5,a5},{500,200,pi/2,0,500,500,0,500,-pi/2,0,500}))
K=[0 0 0 1];


%%
for i=(1:50:499)

d11=(1:1:1+i);
d33=(1:1:1+i);
th22=(0:-0.002093333:-i*0.002093333);
th44=(0:0.00157:i*0.00157);
th55=(0:0.002093333:i*0.002093333);

T01n=double(subs(T01,{d1,a1,al1},{d11(1,i),200,pi/2}))
T02n=double(subs(T02,{d1,a1,al1,th2,a2},{d11(1,i),200,pi/2,th22(1,i),500}))
T03n=double(subs(T03,{d1,a1,al1,th2,a2,d3},{d11(1,i),200,pi/2,th22(1,i),500,d33(1,i)}))
T04n=double(subs(T04,{d1,a1,al1,th2,a2,d3,th4,a4,al4},{d11(1,i),200,pi/2,th22(1,i),500,d33(1,i),th44(1,i),500,-pi/2}))
T05n=double(subs(T05,{d1,a1,al1,th2,a2,d3,th4,a4,al4,th5,a5},{d11(1,i),200,pi/2,th22(1,i),500,d33(1,i),th44(1,i),500,-pi/2,th55(1,i),500}))
K=[0 0 0 1];

T01c=T01n.*K
T02c=T02n.*K
T03c=T03n.*K
T04c=T04n.*K
T05c=T05n.*K
figure(1)
grid on
title('Manipulator')
xlabel('X-axis')
ylabel('Y-axis')
zlabel('Z-axis')
text(double(T04c(1,4)),double(T04c(2,4)),double(T04c(3,4)),'')

plot3([double(T01c(1,4)) double(T02c(1,4))],[double(T01c(2,4)) double(T02c(2,4))],[double(T01c(3,4)) double(T02c(3,4))])
hold on
plot3([double(T02c(1,4)) double(T03c(1,4))],[double(T02c(2,4)) double(T03c(2,4))],[double(T02c(3,4)) double(T03c(3,4))])
hold on
plot3([double(T03c(1,4)) double(T04c(1,4))],[double(T03c(2,4)) double(T04c(2,4))],[double(T03c(3,4)) double(T04c(3,4))])
hold on
plot3([double(T04c(1,4)) double(T05c(1,4))],[double(T04c(2,4)) double(T05c(2,4))],[double(T04c(3,4)) double(T05c(3,4))])
hold on

plot3(double(T01c(1,4)),double(T01c(2,4)),double(T01c(3,4)),'-o')

plot3(double(T02c(1,4)),double(T02c(2,4)),double(T02c(3,4)),'-o')

plot3(double(T03c(1,4)),double(T03c(2,4)),double(T03c(3,4)),'-o')

plot3(double(T04c(1,4)),double(T04c(2,4)),double(T04c(3,4)),'-o')

plot3(double(T05c(1,4)),double(T05c(2,4)),double(T05c(3,4)),'-d')
hold off
axis([-2000 2000 -2000 2000 -2000 2000])
end