Programming Code Structure Assignment

Mechanisms and Robotics

Code :
clc
clear all
syms t1 t2 t3 t4 t5 t6;
DH=[ 0 0 0 t1;
-90*pi/180 0 0 t2;
0 5 0 t3;
-90*pi/180 0 4 t4;
90*pi/180 0 1 t5;
-90*pi/180 0 1 t6;];
j1=[1 1 1 1 1 1];
b1=1;
real_DH=Ss(DH,t1,0,t2,0,t3,0,t4,0,t5,-20*pi/180,t6,0);
p1=[2 2 2];
a1=[0 0 20*pi/180];
Twb=BT(p1,a1);
figure
axis([0 10 0 10 -5 5])
view(3)
grid
camproj orthographic
rotate3d on
RP(real_DH,j1,b1,Twb);
T16=FK(real_DH,1,6);
Tw6=Twb*T16;
T61=IT(T16);
function Tws=BT(p1,a1)
z=a1(3);
x=a1(1);
y=a1(2);
rz=[cos(z) -sin(z) 0
sin(z) cos(z) 0
0 0 1];
ry=[cos(y) 0 sin(y)
0 1 0
-sin(y) 0 cos(y)];
rx=[1 0 0
0 cos(x) -sin(x)
0 sin(x) cos(x)];
rot=rz*ry*rx;
Tws=[rot p1';0 0 0 1];
function Tr=FK(DH,varargin)
if(isempty(varargin))
f=1;
l=size(DH,1);
i1=0;
else

f=varargin{1};
l=varargin{2};
if(varargin{2}>=varargin{1})
f=varargin{1};
l=varargin{2};
i1=0;
else
f=varargin{2};
l=varargin{1};
i1=1;
end
end
for i=f:l
al=DH(i,1);
a=DH(i,2);
d=DH(i,3);
t=DH(i,4);
T(:,:,i)=[cos(t) -sin(t) 0
a;
sin(t)*cos(al) cos(t)*cos(al) -sin(al) -
sin(al)*d;
sin(t)*sin(al) cos(t)*sin(al) cos(al)
cos(al)*d;
0 0 0 1];
end
Tr=eye(4);
for i=f:l
Tr=Tr*T(:,:,i);
end
if(i1)
Tr=IT(Tr);
end
end
function i1=IT(T)
R=T(1:3,1:3);
nR=R';
nP=-nR*T(1:3,4);
i1=[nR nP; 0 0 0 1];
end
function Tr=RP(DH,varargin)
aj1=zeros(1,size(DH,1));
Twb=[1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1];
b1=1;
if(isempty(varargin))
aj1=ones(1,size(DH,1));
else
aj1=varargin{1};
aj1=[aj1 zeros(1,size(DH,1)-length(aj1))];
if(size(varargin,2)==2)
b1=varargin{2};
end
if(size(varargin,2)==3)
b1=varargin{2};
Twb=varargin{3};

end
end
hold on
m1=max(max(DH(:,2:3)));
if1=[m1/10 m1/10 m1/10 1 ; 0 0 0 1];
plot3(if1(:,1),[0;0],[0;0],'r','LineWidth',2)
plot3([0;0],if1(:,2),[0;0],'g','LineWidth',2)
plot3([0;0],[0;0],if1(:,3),'b','LineWidth',2)
ij1=[0 0 0 1 ;0 0 m1/5 1];
for i=1:size(DH,1)
Tr=Twb*FK(DH,1,i);
p2=Tr*[0 0 0 1]';
x(1,i)=p2(1);
y(1,i)=p2(2);
z(1,i)=p2(3);
if(aj1(i))
r1=Tr*ij1';
j3=r1';
plot3(j3(:,1),j3(:,2),j3(:,3),'m:','LineWidth',3)
plot3(x(1,i),y(1,i),z(1,i),'ok','MarkerFaceColor','k')
end
end
plot3(x,y,z,'LineWidth',2)
hold off
end
function Tr=Ss(T,varargin)
op1 = size(varargin,2);
if(mod(op1,2)~=0)
error('There must be even optional arguments')
end
Tr=T;
for i=1:2:op1
Tr=subs(Tr,varargin{i},varargin{i+1},0);
end
Command Window :

Plot:
0
2 4 6
8
10
0
5
10
-5
0
5