Contents
- Clear workspace variables and close all figures
- Constants:
- Driver angle:
- Initial estimations:
- Constraint equations:
- Jacobian:
- Correction:
- Newton-Raphson for the first position:
- visualisation
- Kinematics: driver is rotating with constant angular velocity
- Plot the angles of the bars:
- Animation
- Capturing the animation into an animated GIF
- Computing the velocities and accelerations
- Plot positions, velocities and accelerations
% Kinematics of the four bar mechanisms:
Clear workspace variables and close all figures
close all
clear
clc
Constants:
%the length of four bars L1 = 170; %base L2 = 80; %crank L3 = 200; %connecting rod L4 = 120; %rocker t = 0; %start time of the simulation %To display: the mechanism is defined by four point A B C D (A and D on the base elemens) xA = 0; yA = 0; xD = xA+L1; yD = 0;
Driver angle:
%initial driver link angle: fi2_initial=30*pi/180; % constant angular velocity omega2=10*pi/180;
Initial estimations:
fi2=fi2_initial; fi3=30*pi/180; fi4=230*pi/180;
Constraint equations:
eq(1)=L2*cos(fi2)+L3*cos(fi3)+L4*cos(fi4)-L1;
eq(2)=L2*sin(fi2)+L3*sin(fi3)+L4*sin(fi4);
fprintf('The constraint violations of equation %d is %4.1f n',[[1:2];eq(1:2)]);
The constraint violations of equation 1 is -4.6
The constraint violations of equation 2 is 48.1
Jacobian:
disp('The numeric values of the Jacobian are:')
jac=[-L3*sin(fi3), -L4*sin(fi4);
L3*cos(fi3), L4*cos(fi4)]
The numeric values of the Jacobian are:
jac =
-100.0000 91.9253
173.2051 -77.1345
Correction:
cor=-inv(jac)*eq';
fprintf('The value of the correction term %d is %4.5f n',[[1:2];cor']);
fi3=fi3+cor(1);
fi4=fi4+cor(2);
The value of the correction term 1 is -0.49471
The value of the correction term 2 is -0.48761
Newton-Raphson for the first position:
i = 1; while max(abs(cor))>10e-6 eq(1)=L2*cos(fi2)+L3*cos(fi3)+L4*cos(fi4)-L1; eq(2)=L2*sin(fi2)+L3*sin(fi3)+L4*sin(fi4); jac=[-L3*sin(fi3), -L4*sin(fi4); L3*cos(fi3), L4*cos(fi4)]; cor=-inv(jac)*eq'; fi3=fi3+cor(1); fi4=fi4+cor(2); fprintf('The maximum of the absolute values of the correction term at iteration %d is %4.8f n',[i;max(abs(cor))]); i = i+1; end
The maximum of the absolute values of the correction term at iteration 1 is 0.28653634
The maximum of the absolute values of the correction term at iteration 2 is 0.04072861
The maximum of the absolute values of the correction term at iteration 3 is 0.00020268
The maximum of the absolute values of the correction term at iteration 4 is 0.00000007
visualisation
figure %coordonates of the points: xB=xA+L2*cos(fi2); yB=yA+L2*sin(fi2); xC=xB+L3*cos(fi3); yC=yB+L3*sin(fi3); %lines of the four bars line([xA xB xC xD],[yA yB yC yD],'LineWidth',3,'Color','m') line([xA xD],[yA yD],'LineWidth',5,'Color','g') hold on %the revolute joints plot(xA,yA,'o','LineWidth',2,... 'MarkerSize',6,... 'MarkerEdgeColor','b',... 'MarkerFaceColor','y') plot(xB,yB,'o','LineWidth',2,... 'MarkerSize',6,... 'MarkerEdgeColor','b',... 'MarkerFaceColor','y') plot(xC,yC,'o','LineWidth',2,... 'MarkerSize',6,... 'MarkerEdgeColor','b',... 'MarkerFaceColor','y') plot(xD,yD,'o','LineWidth',2,... 'MarkerSize',6,... 'MarkerEdgeColor','b',... 'MarkerFaceColor','y') axis equal axis([-100 300 -200 200]) title('Four-bar initial position') xlabel('x [mm]') ylabel('y [mm]') hold off
Kinematics: driver is rotating with constant angular velocity
%one full rotations t_final = 2*pi/omega2; time=[]; fi2_result=[]; fi3_result=[]; fi4_result=[]; cor=[1;1]; %re-initialising to avoid skipping the loop for t=0:0.2:t_final %fprintf('Current time in the simulation is %4.2f sec. n',t); fi2=fi2_initial+t*omega2; cor=[1;1]; %re-initialising to avoid skipping the loop while max(abs(cor))>10e-6 eq(1)=L2*cos(fi2)+L3*cos(fi3)+L4*cos(fi4)-L1; eq(2)=L2*sin(fi2)+L3*sin(fi3)+L4*sin(fi4); jac=[-L3*sin(fi3), -L4*sin(fi4); L3*cos(fi3), L4*cos(fi4)]; cor=-inv(jac)*eq'; fi3=fi3+cor(1); fi4=fi4+cor(2); end time=[time,t]; fi2_result=[fi2_result,fi2]; fi3_result=[fi3_result,fi3]; fi4_result=[fi4_result,fi4]; end
Plot the angles of the bars:
figure
subplot(3,1,1)
plot(time,(180/pi)*fi2_result,'g','LineWidth',2)
xlabel('Time [s]')
ylabel('phi_2 [circ]')
grid on
subplot(3,1,2)
plot(time,(180/pi)*fi3_result,'r','LineWidth',2)
xlabel('Time [s]')
ylabel('phi_3 [circ]')
grid on
subplot(3,1,3)
plot(time,(180/pi)*fi4_result,'b','LineWidth',2)
xlabel('Time [s]')
ylabel('phi_4 [circ]')
grid on
Animation
%Drawing the four-bar mechanisms at the computed positions figure nrframe=size(fi2_result,2); for i=1:nrframe clf xB=xA+L2*cos(fi2_result(i)); yB=yA+L2*sin(fi2_result(i)); xC=xB+L3*cos(fi3_result(i)); yC=yB+L3*sin(fi3_result(i)); line([xA xB xC xD],[yA yB yC yD],'LineWidth',3,'Color','m') line([xA xD],[yA yD],'LineWidth',5,'Color','g') hold on plot(xA,yA,'o','LineWidth',2,'MarkerSize',6,... 'MarkerEdgeColor','b','MarkerFaceColor','y') plot(xB,yB,'o','LineWidth',2,'MarkerSize',6,... 'MarkerEdgeColor','b','MarkerFaceColor','y') plot(xC,yC,'o','LineWidth',2,'MarkerSize',6,... 'MarkerEdgeColor','b','MarkerFaceColor','y') plot(xD,yD,'o','LineWidth',2,'MarkerSize',6,... 'MarkerEdgeColor','b','MarkerFaceColor','y') axis equal axis([-100 300 -200 200]) title('Four-bar simulation') xlabel('x [mm]') ylabel('y [mm]') hold off pause(0.005) drawnow limitrate end
Capturing the animation into an animated GIF
filename='animated_fourbar.gif'; figure for i=1:3:nrframe clf xB=xA+L2*cos(fi2_result(i)); yB=yA+L2*sin(fi2_result(i)); xC=xB+L3*cos(fi3_result(i)); yC=yB+L3*sin(fi3_result(i)); line([xA xB xC xD],[yA yB yC yD],'LineWidth',3,'Color','m') line([xA xD],[yA yD],'LineWidth',5,'Color','g') hold on plot(xA,yA,'o','LineWidth',2,'MarkerSize',6,... 'MarkerEdgeColor','b','MarkerFaceColor','y') plot(xB,yB,'o','LineWidth',2,'MarkerSize',6,... 'MarkerEdgeColor','b','MarkerFaceColor','y') plot(xC,yC,'o','LineWidth',2,'MarkerSize',6,... 'MarkerEdgeColor','b','MarkerFaceColor','y') plot(xD,yD,'o','LineWidth',2,'MarkerSize',6,... 'MarkerEdgeColor','b','MarkerFaceColor','y') axis equal axis([-100 300 -200 200]) title('Four-bar simulation') xlabel('x [mm]') ylabel('y [mm]') hold off % Get figure size and sive it into the file pos = get(gcf,'Position'); width = pos(3); height = pos(4); f = getframe(gcf); framemovie(i)=f; im = frame2im(f); [imind,cm] = rgb2ind(im,256); if i == 1 imwrite(imind,cm,filename,'gif', 'Loopcount',inf); else imwrite(imind,cm,filename,'gif','WriteMode','append'); end end
Computing the velocities and accelerations
omega2_result=[]; omega3_result=[]; omega4_result=[]; epsilon2_result=[]; epsilon3_result=[]; epsilon4_result=[]; cor=[1;1]; %re-initialising to avoid skipping the loop for t=0:0.2:t_final fi2=fi2_initial+t*omega2; cor=[1;1]; while max(abs(cor))>10e-6 eq(1)=L2*cos(fi2)+L3*cos(fi3)+L4*cos(fi4)-L1; eq(2)=L2*sin(fi2)+L3*sin(fi3)+L4*sin(fi4); jac=[-L3*sin(fi3), -L4*sin(fi4); L3*cos(fi3), L4*cos(fi4)]; cor=-inv(jac)*eq'; fi3=fi3+cor(1); fi4=fi4+cor(2); end %time derivative of constraint equations: deq_dt(1)=-L2*sin(fi2)*omega2; deq_dt(2)= L2*cos(fi2)*omega2; vel=-inv(jac)*deq_dt'; omega3=vel(1); omega4=vel(2); %second time derivative of constraint equations: deq_dt2(1)=-L2*cos(fi2)*omega2*omega2-L3*cos(fi3)*omega3*omega3-L4*cos(fi4)*omega4*omega4; deq_dt2(2)=-L2*sin(fi2)*omega2*omega2-L3*sin(fi3)*omega3*omega3-L4*sin(fi4)*omega4*omega4; acce=-inv(jac)*deq_dt2'; epsilon3=acce(1); epsilon4=acce(2); omega2_result=[omega2_result,omega2]; omega3_result=[omega3_result,omega3]; omega4_result=[omega4_result,omega4]; epsilon2_result=[epsilon2_result,0]; epsilon3_result=[epsilon3_result,epsilon3]; epsilon4_result=[epsilon4_result,epsilon4]; end
Plot positions, velocities and accelerations
figure
subplot(3,1,1)
plot(time,(180/pi)*fi2_result,'g','LineWidth',2)
xlabel('Time [s]')
ylabel('phi_2 [circ]')
grid on
title('Position')
subplot(3,1,2)
plot(time,(180/pi)*omega2_result,'r','LineWidth',2)
xlabel('Time [s]')
ylabel('omega_2 [circ/s]')
grid on
title('Velocity')
subplot(3,1,3)
plot(time,(180/pi)*epsilon2_result,'b','LineWidth',2)
xlabel('Time [s]')
ylabel('epsilon_2 [circ/s^2]')
grid on
title('Acceleration')
figure
subplot(3,1,1)
plot(time,(180/pi)*fi3_result,'g','LineWidth',2)
xlabel('Time [s]')
ylabel('phi_3 [circ]')
grid on
title('Position')
subplot(3,1,2)
plot(time,(180/pi)*omega3_result,'r','LineWidth',2)
xlabel('Time [s]')
ylabel('omega_3 [circ/s]')
grid on
title('Velocity')
subplot(3,1,3)
plot(time,(180/pi)*epsilon3_result,'b','LineWidth',2)
xlabel('Time [s]')
ylabel('epsilon_3 [circ/s^2]')
grid on
title('Acceleration')
figure
subplot(3,1,1)
plot(time,(180/pi)*fi4_result,'g','LineWidth',2)
xlabel('Time [s]')
ylabel('phi_4 [circ]')
grid on
title('Position')
subplot(3,1,2)
plot(time,(180/pi)*omega4_result,'r','LineWidth',2)
xlabel('Time [s]')
ylabel('omega_4 [circ/s]')
grid on
title('Velocity')
subplot(3,1,3)
plot(time,(180/pi)*epsilon4_result,'b','LineWidth',2)
xlabel('Time [s]')
ylabel('epsilon_4 [circ/s^2]')
grid on
title('Acceleration')
