workspace_kod

% this script determines a Homogeneous Transformation HT matrix
clear all
% declaration of symbols
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;
T35=A4*A5
T05=A1*A2*A3*A4*A5;

%%
% Horizontal workspace
for i=(1:1:100)

d11=800;
th22=(-1.7444:0.034888:1.7444);
a33=600;


T01n=double(subs(T01,{d1,a1},{d11,500}))
T02n=double(subs(T02,{d1,a1,th2},{d11,500,th22(1,i)}))
T03n=double(subs(T03,{d1,a1,th2,a3,al3},{d11,500,th22(1,i),a33,-pi/2}))
T04n=double(subs(T04,{d1,a1,th2,a3,al3,th4,a4,al4},{d11,500,th22(1,i),a33,-pi/2,0,500,pi/2}))
T05n=double(subs(T05,{d1,a1,th2,a3,al3,th4,a4,al4,al5},{d11,500,th22(1,i),a33,-pi/2,0,500,pi/2,0}))
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))])

plot3(500, 0, 0,'-x')
plot3(double(T01c(1,4)),double(T01c(2,4)),double(T01c(3,4)),'-o')
hold on
plot3(double(T02c(1,4)),double(T02c(2,4)),double(T02c(3,4)),'-o')
hold on
plot3(double(T03c(1,4)),double(T03c(2,4)),double(T03c(3,4)),'-o')
hold on
plot3(double(T04c(1,4)),double(T04c(2,4)),double(T04c(3,4)),'-o')
hold on
plot3(double(T05c(1,4)),double(T05c(2,4)),double(T05c(3,4)),'-d')
hold on
axis([-2000 2000 -2000 2000 -2000 2000])
end

%Vertical workspace

for i=(1:1:100)

d11=(100:7:100+7*i);
th22=0;
a33=(100:5:100+5*i);


T01n=double(subs(T01,{d1,a1},{d11(1,i),500}))
T02n=double(subs(T02,{d1,a1,th2},{d11(1,i),500,th22}))
T03n=double(subs(T03,{d1,a1,th2,a3,al3},{d11(1,i),500,th22,a33(1,i),-pi/2}))
T04n=double(subs(T04,{d1,a1,th2,a3,al3,th4,a4,al4},{d11(1,i),500,th22,a33(1,i),-pi/2,0,500,pi/2}))
T05n=double(subs(T05,{d1,a1,th2,a3,al3,th4,a4,al4,al5},{d11(1,i),500,th22,a33(1,i),-pi/2,0,500,pi/2,0}))
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))])

plot3(500, 0, 0,'-x')
plot3(double(T01c(1,4)),double(T01c(2,4)),double(T01c(3,4)),'-o')
hold on
plot3(double(T02c(1,4)),double(T02c(2,4)),double(T02c(3,4)),'-o')
hold on
plot3(double(T03c(1,4)),double(T03c(2,4)),double(T03c(3,4)),'-o')
hold on
plot3(double(T04c(1,4)),double(T04c(2,4)),double(T04c(3,4)),'-o')
hold on
plot3(double(T05c(1,4)),double(T05c(2,4)),double(T05c(3,4)),'-d')
hold on
axis([-2000 2000 -2000 2000 -2000 2000])
end