MATLAB code for the Position & Velocity Calcs and Simulations

clc;
clear all;
close all;

video_record = false;
vid_name = 'robotic_gripper_sim.avi';

q1a = 284.67;
%Mechanism A link length of five-bar
LA = [2.5332, 1.4921, 1.1811, 0.7874, 1.3671];
theta1 = 284.6;

%Mechanism B links, last link length is to point P
LB = [1.1815, 2.2885, 1.0206, 1.9685, 2.6202];
alpha = 122.9;
R = [cosd(alpha) -sind(alpha); sind(alpha) cosd(alpha)];
R_inv = R';
link5a_offset_angle = 110;
delta = -1;
qP = 35.67;

syms theta4 theta5 w4 w5;

LA2_start = 0.6;
LA2_end = 2.2;
step = 0.05;
LA2_all = LA2_start:step:LA2_end;

%TODO convert this to linear speed based on rotation of motor
RPS = 4;
TPI = 20;
% evaluates to 0.2;
LA_2dot = 1/TPI*RPS;

%split the angles of the two mechanisms up for simplicity
qA = zeros(length(LA2_all),5);
omegaA = qA;
L2A = zeros(length(LA2_all),2);
L3A = L2A;
L4A = L2A;
L1A = L2A;

omegaA(:,1) = 0;
omegaA(:,2) = 0;
omegaA(:,3) = 0;

qB = zeros(length(LA2_all),5);
qB_global = qB;
omegaB = zeros(length(LA2_all),5);

L1B = zeros(length(LA2_all),2);
L2B = L1B;
L3B = L1B;
LPB = L1B;
LPB_mag = zeros(length(LA2_all),1);
LPB_vel = L1B;
LPB_velMag = LPB_mag;

if video_record == true
video = VideoWriter(vid_name); %create the video object
open(video);
end

for i = 1:length(LA2_all)

LA(2) = LA2_all(i);

% Calling the function that calculates fourbar positions [q1, q2, q3, q4]
qA(i,1) = 284.67;
qA(i,2) = 0;
qA(i,3) = 270;

q1a = wrapTo360(qA(i,1));
q2a = wrapTo360(qA(i,2));
q3a = wrapTo360(qA(i,3));

%Solve for theta4 and theta5 of Mechanism A
eqq1 = LA(1)*cosd(theta1) + LA(5)*cosd(theta5) - LA(4)*cosd(theta4) - LA(2) == 0;
eqq2 = LA(1)*sind(theta1) + LA(5)*sind(theta5) - LA(4)*sind(theta4) + LA(3) == 0;

Y = vpasolve([eqq1, eqq2], [theta4,theta5],[200;80]);
qA(i,4) = Y.theta4;
qA(i,5) = Y.theta5;

q4a = wrapTo360(qA(i,4));
q5a = wrapTo360(qA(i,5));

%Solve for w4 and w5;
eqq3 = -LA(5)*w5*sind(q5a) + LA(4)*w4*sind(q4a) - LA_2dot == 0;
eqq4 = LA(5)*w5*cosd(q5a) - LA(4)*w4*cosd(q4a) == 0;

Z = vpasolve([eqq3, eqq4], [w4,w5],[0.2;-0.5]);
omegaA(i,4) = double(Z.w4);
omegaA(i,5) = Z.w5;

omega4A = omegaA(i,4);
omega5A = omegaA(i,5);

%Construct Position Vectors
origin = [0; 0];
L1A(i,:) = [LA(1)*cosd(q1a); LA(1)*sind(q1a)];
% L_A = [LA(2)*cosd(q2); -L2y];
L2A(i,:) = [LA(2)*cosd(q2a); LA(2)*sind(q2a)];
L3A(i,:) = [L2A(i,1) + LA(3)*cosd(q3a);L2A(i,2) + LA(3)*sind(q3a)];
% can do the end point of L4A (point C) by 1 of these 2 ways below
% L4A(i,:) = L3A(i,:) + [LA(4)*cosd(q4a); LA(4)*sind(q4a)];
L4A(i,:) = [L1A(i,1) + LA(5)*cosd(q5a);L1A(i,2) + LA(5)*sind(q5a)];


%2nd Mechanism
q2b = q5a + link5a_offset_angle - alpha;
qB(i,1:4) = fourbarpos_deg(LB,0,q2b,delta);
q1b = wrapTo360(qB(i,1));
q2b = wrapTo360(qB(i,2));
q3b = wrapTo360(qB(i,3));
q4b = wrapTo360(qB(i,4));

omega2B = omega5A;
omegaB(i,1:4) = fourbarVel_deg(LB,qB(i,1:4),omega2B);

omega3B = omegaB(i,3);
omega4B = omegaB(i,4);

%Construct Mechanism B Position Vectors
L1B(i,:) = [LB(1)*cosd(q1b); LB(1)*sind(q1b)];
L2B(i,:) = [LB(2)*cosd(q2b); LB(2)*sind(q2b)];
% can do the end point of L3B (point B) by 1 of these 2 ways below
L3B(i,:) = [LB(3)*cosd(q3b); LB(3)*sind(q3b)];
% L3B(i,:) = [LB(4)*cosd(q4b); LB(4)*sind(q4b)];

%Construct Position Vector of Point P (Gripper)
LPB(i,:) = [LB(2)*cosd(q2b) + LB(5)*cosd(q3b+qP); LB(2)*sind(q2b) + LB(5)*sind(q3b+qP)];
LPB_mag(i) = norm(LPB(i,:));

%Calculate q5 using dot product
qB(i,5) = acosd(dot([1;0],LPB(i,:))/(LPB_mag(i)*1));

%Construct Velocity Vector of Point P (Gripper
LPB_xVel = -LB(2)*omega2B*sind(q2b) - LPB_mag(i)*omega3B*sind(q3b+qP);
LPB_yVel = LB(2)*omega2B*cosd(q2b) + LPB_mag(i)*omega3B*cosd(q3b+qP);
LPB_vel(i,:) = [LPB_xVel; LPB_yVel];
LPB_velMag(i) = norm(LPB_vel(i));
omegaB(i,5) = LPB_velMag(i)/LPB_mag(i);

%Convert local Mechanism B vectors back into global frame
L1B(i,:) = R*L1B(i,:)';
L2B(i,:) = R*L2B(i,:)';
L3B(i,:) = R*L3B(i,:)';
LPB(i,:) = R*LPB(i,:)';
LPB_vel(i,:) = R*LPB_vel(i,:)';
%convert thetas of mech B to global coords
qB_global = qB + alpha;

%Build up vector from global origin by adding L1A into the mix
%(position vector from O2 to O4 of Mechanism A)
L1B(i,:) = [L1A(i,1) + L1B(i,1); L1A(i,2) + L1B(i,2)];
L2B(i,:) = [L1A(i,1) + L2B(i,1); L1A(i,2) + L2B(i,2)];
L3B(i,:) = [L2B(i,1) + L3B(i,1); L2B(i,2) + L3B(i,2)];
% L3B(i,:) = [L1B(i,1) + L3B(i,1); L1B(i,2) + L3B(i,2)];
LPB(i,:) = [L1A(i,1) + LPB(i,1); L1A(i,2) + LPB(i,2)];

end

%Plottting
plot_links = true;
if plot_links == true

for i = 1:length(LA2_all)
figure(1)
set(gcf,'position',[0 10 1300 900])
plot(origin(1),origin(2),'ro','linewidth',3)
hold on
plot(L1A(i,1),L1A(i,2),'ro','linewidth',3)
plot(L2A(i,1),L2A(i,2),'ro','linewidth',3)
plot(L3A(i,1),L3A(i,2),'ro','linewidth',3)
plot(L4A(i,1),L4A(i,2),'bx','linewidth',2)
% 2nd mechanism
plot(L1B(i,1),L1B(i,2),'bx','linewidth',2)
plot(L2B(i,1),L2B(i,2),'bx','linewidth',2)
plot(L3B(i,1),L3B(i,2),'bx','linewidth',2)
plot(LPB(i,1),LPB(i,2),'bx','linewidth',2)

% 1st Mechanism
line([origin(1) L2A(i,1)], [origin(2) L2A(i,2)],'Color','yellow','linewidth',3)
line([L2A(i,1) L3A(i,1)], [L2A(i,2) L3A(i,2)],'Color','red','linewidth',3)
line([L3A(i,1) L4A(i,1)], [L3A(i,2) L4A(i,2)],'Color','blue','linewidth',3)
line([L1A(i,1) L4A(i,1)], [L1A(i,2) L4A(i,2)],'Color','green','linewidth',3)
% ground link
line([origin(1) L1A(i,1)], [origin(2) L1A(i,2)],'Color','black','linewidth',3,'LineStyle','--')
% 2nd Mechanism
line([L1A(i,1) L2B(i,1)], [L1A(i,2) L2B(i,2)],'Color','red','linewidth',3)
line([L2B(i,1) L3B(i,1)], [L2B(i,2) L3B(i,2)],'Color','blue','linewidth',3)
line([L1B(i,1) L3B(i,1)], [L1B(i,2) L3B(i,2)],'Color','green','linewidth',3)
% ground link
line([L1A(i,1) L1B(i,1)], [L1A(i,2) L1B(i,2)],'Color','black','linewidth',3)
% point of interest, P
line([L2B(i,1) LPB(i,1)], [L2B(i,2) LPB(i,2)],'Color','yellow','linewidth',3,'LineStyle','--')
line([L3B(i,1) LPB(i,1)], [L3B(i,2) LPB(i,2)],'Color','yellow','linewidth',3,'LineStyle','--')
% plot trajectory
Px(i) = LPB(i,1);
Py(i) = LPB(i,2);
Plot_Traj = plot(Px,Py,'--g','LineWidth',1);

title('Robotic Gripper (one Five-bar, one Four-bar)','fontsize',15)
xlabel('x','fontsize',15)
ylabel('y','fontsize',15)
axis ([-5 5 -5 5]); %equal
pause(0.5)
delete(Plot_Traj);
if i < length(LA2_all)
hold off
else
plot(Px,Py,'--g','LineWidth',1);
hold on
end

if video_record == true
frame = getframe(gcf);
writeVideo(video,frame); %write the image to file
end

end

end

plot_stats = true;
if plot_stats == true
figure(3)
plot(LA2_all, qA(:,4),'linewidth',2)
hold on;
plot(LA2_all, qA(:,5),'linewidth',2)
title('Mechanism A Links Angular Position');
xlabel('Theta 2 (deg)');
ylabel('Degrees');
legend('Link 4 Angle','Link 5 Angle');

figure(4)
plot(LA2_all, qB_global(:,3),'linewidth',2)
hold on;
plot(LA2_all, qB_global(:,4),'linewidth',2)
plot(LA2_all, qB_global(:,5),'linewidth',2)
title('Mechanism B Links Angular Position');
xlabel('Theta 2 (deg)');
ylabel('Degrees');
legend('Link 3 Angle','Link 4 Angle','Link O2_P Angle');

figure(5)
plot(LA2_all, omegaA(:,4),'linewidth',2)
hold on;
plot(LA2_all, omegaA(:,5),'linewidth',2)
title('Mechanism A Links Angular Velocity');
xlabel('Theta 2 (deg)');
ylabel('Omega (rad/s)');
legend('Link 4 Ang Vel','Link 5 Ang Vel');

figure(6)
plot(LA2_all, omegaB(:,3),'linewidth',2)
hold on;
plot(LA2_all, omegaB(:,4),'linewidth',2)
plot(LA2_all, omegaB(:,5),'linewidth',2)
title('Mechanism B Links Angular Velocity');
xlabel('Theta 2 (deg)');
ylabel('Omega (rad/s)');
legend('Link 3 Ang Vel','Link 4 Ang Vel','Link O2_P Ang Vel');
end

if video_record == true
close(video)
end

Bill of Materials