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)
...
MATLAB code for the Velocity and Acceleration of Point P - TOADD
...
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
Item | Link | Price for All | Notes |
36 M5 Bearings and associated fasteners (bolts and nuts) | McMaster | ||
16 3D printed links (8 of which are mirrored versions of the first 8) | PLA filament | ||
AX-12 Dynamixel Servo | |||
U2D2 Converter Board | |||
U2D2 Power Hub | |||
2 TTL Cables | |||
Bracket & Associated Hardware for attaching servo | |||
1/4-20" threaded rod | |||
3d printed threaded rod to servo horn couple |