:"%%
% Robotics Course, Professor: Prof. Hamid D. Taghirad
% Aras.kntu.ac.ir/education/robotics
% Copyright Ali.Hassani 2020
%
% This program Calculate Work-Space of 3-DOF parallelogram-based robot
%%
clear;
clc;
%% Structure Parameters
L1=0.4;L3=0.25; %(Optional Values)
%% Workspace
i=0; N=100; %(Optional Values)
for theta1=linspace(-pi/4,pi/4,N)%(Optional Values)
for theta2=linspace(-pi/4+pi/2,pi/4+pi/2,N) %(Optional Values)
for d3=linspace(0,0.03,N) %(Optional Values)
i=i+1;
%% FW Equations
px(i)=1.0*d3*cos(theta1)*sin(theta2);
py(i)=1.0*d3*sin(theta1)*sin(theta2);
pz(i)=1.0*L1 + 1.0*L3 + 1.0*d3*cos(theta2);
end
end
end
%% Plot
figure(1)
scatter3(pz,py,px,'markeredgecolor','r','MarkerFaceColor','blue')
xlabel('z (m)')
ylabel('y (m)')
zlabel('x (m)')
suptitle('WorkSpace of Robot')"
"%%
% Robotics Course, Professor: Prof. Hamid D. Taghirad
% Aras.kntu.ac.ir/education/robotics
% Copyright Ali.Hassani 2020
%
% This code provides a better representation of the vector symbolic formulas in MATLAB
%%
function [P_real] = Vector_Vpa(P,n)
for i=1:n
[C,T] = coeffs(P(i));
c=vpa(C,2);
if abs(c) < 0.001
c=0;
end
P3_real(i,1)=dot(c,T);
end
P_real=vpa(P3_real,3);
end"
"%%
% Robotics Course, Professor: Prof. Hamid D. Taghirad
% Aras.kntu.ac.ir/education/robotics
% Copyright Ali.Hassani 2020
%
% This code provides the various kinematic parts of 3-DOF parallelogram-based robot
%%
clear;
clc;
syms theta1 theta2 d3 L1 L3
%% DH Table
T1=DH(0,-pi/2,L1+L3,theta1); T1=Matrix_Vpa(T1,4,4);
T2=DH(0,+pi/2,0,theta2); T2=Matrix_Vpa(T2,4,4);
T3=DH(0,0,d3,0);
T_Final=T1*(T2*T3);
%% Forward Kinematics
P_DH=T_Final(1:3,4);
R_DH=T_Final(1:3,1:3);
%% Forward Kinematics (Screw)
S1=SR(0,0,+1,0,0,0,theta1,0);
S2=SR(0,+1,0,0,0,L1+L3,theta2,0);
S3=SR(0,0,1,0,0,0,0,0);
Screw=S1*(S2*S3);
U0=[1;0;0];V0=[0;1;0];W0=[0;0;1];P0=[0;0;L1+L3+d3];Ze=[0,0,0];
S0=[U0,V0,W0,P0;Ze,1];
S_Final=Screw*S0;
R_SR=S_Final(1:3,1:3);
P_SR=S_Final(1:3,4);P_SR=simplify(P_SR);
%% Verification
E_Position=simplify(P_DH-P_SR)
E_Rotation=simplify(R_DH-R_SR)
%% Final_Step: Export Forward Kinematics to a Functions!
"
"%%
% Robotics Course, Professor: Prof. Hamid D. Taghirad
% Aras.kntu.ac.ir/education/robotics
% Copyright Ali.Hassani 2020
%
% This program Defines Screw Parameters
%%
function [S] = SR(s__x,s__y,s__z,s__ox,s__oy,s__oz,theta,t)
S=[(s__x ^ 2 - 1) * (0.1e1 - cos(theta)) + 0.1e1 s__x * s__y * (0.1e1 - cos(theta)) - s__z * sin(theta) s__x * s__z * (0.1e1 - cos(theta)) + s__y * sin(theta) (t * s__x) - s__ox * (s__x ^ 2 - 1) * (0.1e1 - cos(theta)) - s__oy * (s__x * s__y * (0.1e1 - cos(theta)) - s__z * sin(theta)) - s__oz * (s__x * s__z * (0.1e1 - cos(theta)) + s__y * sin(theta)); s__x * s__y * (0.1e1 - cos(theta)) + s__z * sin(theta) (s__y ^ 2 - 0.1e1) * (0.1e1 - cos(theta)) + 0.1e1 s__y * s__z * (0.1e1 - cos(theta)) - s__x * sin(theta) t * s__y - s__ox * (s__x * s__y * (0.1e1 - cos(theta)) + s__z * sin(theta)) - s__oy * (s__y ^ 2 - 0.1e1) * (0.1e1 - cos(theta)) - s__oz * (s__y * s__z * (0.1e1 - cos(theta)) - s__x * sin(theta)); s__x * s__z * (0.1e1 - cos(theta)) - s__y * sin(theta) s__y * s__z * (0.1e1 - cos(theta)) + s__x * sin(theta) (s__z ^ 2 - 0.1e1) * (0.1e1 - cos(theta)) + 0.1e1 t * s__z - s__ox * (s__x * s__z * (0.1e1 - cos(theta)) - s__y * sin(theta)) - s__oy * (s__y * s__z * (0.1e1 - cos(theta)) + s__x * sin(theta)) - s__oz * (s__z ^ 2 - 0.1e1) * (0.1e1 - cos(theta)); 0 0 0 1;];
end
"
"%%
% Robotics Course, Professor: Prof. Hamid D. Taghirad
% Aras.kntu.ac.ir/education/robotics
% Copyright Ali.Hassani 2020
%
% This code provides a better representation of matrix symbolic formulas, which are numerically defined.
%%
function [T_real] = Matrix_Vpa_Numeric(T3,n,m)
for i=1:n
for j=1:m
C = T3(i,j);
c=vpa(C,2);
if abs(c) < 0.001
c=0;
end
T3_real(i,j)=c;
end
end
T_real=vpa(T3_real,3);
end"
"%%
% Robotics Course, Professor: Prof. Hamid D. Taghirad
% Aras.kntu.ac.ir/education/robotics
% Copyright Ali.Hassani 2020
%
% This code provides a better representation of the matrix symbolic formulas in MATLAB
%%
function [T_real] = Matrix_Vpa(T3,n,m)
for i=1:n
for j=1:m
[C,T] = coeffs(T3(i,j));
c=vpa(C,2);
if abs(c) < 0.001
c=0;
end
T3_real(i,j)=dot(c,T);
end
end
T_real=vpa(T3_real,3);
end"
"%%
% Robotics Course, Professor: Prof. Hamid D. Taghirad
% Aras.kntu.ac.ir/education/robotics
% Copyright Ali.Hassani 2020
%
% This code provides the kinematic verification of 3-DOF parallelogram-based robot
%%
clear;
clc;
i=1;
%% Structure Parameters
L1=0.4; L3=0.25; Structual_Parameters=[L1;L3]; %(Optional Values)
omega=1*pi; %(Optional Values)
%% Kinematic Verification Loop
for t=0:0.005:1
%Trajectory Define
theta1=(pi/4)*sin(omega*t); %(Optional Values)
theta2=(pi/4)*sin(omega*t); %(Optional Values)
d3=0.03*sin(omega*t)+0.03; %(Optional Values)
%% Forward_Kinemtic:
[P,R] = FW(theta1,theta2,d3,Structual_Parameters);
%% Inverse_Kinematic:
[THETA1,THETA2,D3] = IK(P,R,Structual_Parameters);
%% Error
Error_Theta1(i)=theta1-THETA1;
Error_Theta2(i)=theta2-THETA2;
Error_Linear3(i)=d3-D3;
i=i+1;
end
%% Verification Plot
t=0:0.005:1;
subplot(311)
plot(t,Error_Theta1,'r*')
grid on;
xlabel('Time(s)')
ylabel('E_{\theta_1}')
subplot(312)
plot(t,Error_Theta2,'b+')
grid on;
xlabel('Time(s)')
ylabel('E_{\theta_2}')
subplot(313)
plot(t,Error_Linear3,'ko')
grid on;
xlabel('Time(s)')
ylabel('E_{d_3}')
suptitle('Kinematic Verification')"
"%%
% Robotics Course, Professor: Prof. Hamid D. Taghirad
% Aras.kntu.ac.ir/education/robotics
% Copyright Ali.Hassani 2020
%
% This program Defines Inverse Kinematics of 3-DOF parallelogram-based robot
%%
function [THETA1,THETA2,D3] = IK(P,R,Structual_Parameters)
L1=Structual_Parameters(1);
L3=Structual_Parameters(2);
x=P(1); y=P(2); z=P(3);
THETA1=atan2(y,x);
THETA2=atan2(y,sin(THETA1)*(z-L1-L3));
D3=(z-L1-L3)/(cos(THETA2));
end"
"%%
% Robotics Course, Professor: Prof. Hamid D. Taghirad
% Aras.kntu.ac.ir/education/robotics
% Copyright Ali.Hassani 2020
%
% This program Defines Forward Kinematics of 3-DOF parallelogram-based robot
%%
function [P,R] = FW(theta1,theta2,d3,Structual_Parameters)
L1=Structual_Parameters(1);
L3=Structual_Parameters(2);
P=[1.0*d3*cos(theta1)*sin(theta2);1.0*d3*sin(theta1)*sin(theta2);1.0*L1 + 1.0*L3 + 1.0*d3*cos(theta2)];
R=[1.0*cos(theta1)*cos(theta2), -1.0*sin(theta1), 1.0*cos(theta1)*sin(theta2);1.0*cos(theta2)*sin(theta1), 1.0*cos(theta1), 1.0*sin(theta1)*sin(theta2);-1.0*sin(theta2),0, 1.0*cos(theta2)];
end"
"%%
% Robotics Course, Professor: Prof. Hamid D. Taghirad
% Aras.kntu.ac.ir/education/robotics
% Copyright Ali.Hassani 2020
%
% This program Defines DH Parameters
%%
function [T] = DH(a,alpha,d,theta)
T=[cos(theta) -sin(theta) * cos(alpha) sin(theta) * sin(alpha) a * cos(theta); sin(theta) cos(theta) * cos(alpha) -cos(theta) * sin(alpha) a * sin(theta); 0 sin(alpha) cos(alpha) d; 0 0 0 1;];
end"