Scribd
Upload
37 views

MECANISMOS

The document contains code for several exercises involving kinematic analysis of planar linkages. In Exercise 1, the code calculates forward kinematics for a 4-bar linkage by determining joint angles based on input link lengths and the reference angle, and plots the linkage configuration. It also calculates positions where the linkage becomes singular. Exercise 2 extends this to include a floating link and determines its position. Exercise 3 analyzes a 5-bar linkage. Exercise 5 sets up matrices to systematically solve the forward kinematics over a range of input angles.

Uploaded by

Andres Camilo Hernandez Ruiz
Copyright
© © All Rights Reserved
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd
37 views

MECANISMOS

The document contains code for several exercises involving kinematic analysis of planar linkages. In Exercise 1, the code calculates forward kinematics for a 4-bar linkage by determining joint angles based on input link lengths and the reference angle, and plots the linkage configuration. It also calculates positions where the linkage becomes singular. Exercise 2 extends this to include a floating link and determines its position. Exercise 3 analyzes a 5-bar linkage. Exercise 5 sets up matrices to systematically solve the forward kinematics over a range of input angles.

Uploaded by

Andres Camilo Hernandez Ruiz
Copyright
© © All Rights Reserved
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd
You are on page 1/ 8

%Ejercicio1

clc;
clear all;
d=87;
a=49;
b=100;
c=153;
for theta2=(0:10:360);
theta1=0;
k1=d/a;
k2=d/c;
k3=((a^2)-(b^2)+(c^2)+(d^2))/(2*a*c);
k4=d/b;
k5=((c^2)-(d^2)-(a^2)-(b^2))/(2*a*b);
D=cosd(theta2)-k1+k4*cosd(theta2)+k5;
E=-2*sind(theta2);
F=k1+(k4-1)*cosd(theta2)+k5;
A=cosd(theta2)-k1-k2*cosd(theta2)+k3;
B=-2*sind(theta2);
C=k1-(k2+1)*cosd(theta2)+k3;
theta1=0;
theta4=2*atand((-B-sqrt((B^2)-4*A*C))/(2*A));%abierto -
theta3=2*atand((-E-sqrt((E^2)-4*D*F))/(2*D));
% theta4=2*atand((-B+sqrt((B^2)-4*A*C))/(2*A)); %cruzado +
%theta3=2*atand((-E+sqrt((E^2)-4*D*F))/(2*D));
T=[cosd(121) sind(121); -sind(121) cosd(121)];
L1=[d*cosd(theta1);d*sind(theta1)];
L2=[a*cosd(theta2); a*sind(theta2)];
L3=[b*cosd(theta3);b*sind(theta3)];
L4=[c*cosd(theta4);c*sind(theta4)];
IL1=-L1;
IL2=-L2;
IL3=-L3;
IL4=-L4;
R1=T*L1;
R2=T*L2;
R3=T*L3;
R4=T*L4;
inR1=-R1;
inR2=-R2;
inR3=-R3;
inR4=-R4;
plot([0 inR1(1)],[0 inR1(2)],'o-','linewidth',2); hold on
plot([0 R2(1)],[0 R2(2)],'o-','linewidth',2); hold on
plot([R2(1) R2(1)+R3(1)],[R2(2) R2(2)+R3(2)],'o-','linewidth',2); hold on
plot([R2(1)+R3(1) R2(1)+R3(1)-R4(1)],[R2(2)+R3(2) R2(2)+R3(2)-
R4(2)],'o-','linewidth',2); hold on
plot([0 inR2(1)],[0 inR2(2)],'o-','linewidth',2); hold on
plot([inR2(1) inR2(1)+inR3(1)],[inR2(2)
inR2(2)+inR3(2)],'o-','linewidth',2); hold on
plot([inR2(1)+inR3(1) inR2(1)+inR3(1)-inR4(1)],[inR2(2)+inR3(2)
inR2(2)+inR3(2)-inR4(2)] ...
,'o-','linewidth',2); hold on
plot([0 R1(1)],[0 R1(2)],'o-','linewidth',2); hold off
grid on
axis([-110 110 -110 110]);
pause(0.1);
end
%Posiciones de agarrotamiento%

Arg_1 = (a^2+d^2-b^2-c^2)/(2*a*d) + (b*c)/(a*d);


Arg_2 = (a^2+d^2-b^2-c^2)/(2*a*d) + (b*c)/(a*d);

if (0<=Arg_1) && (Arg_1<=180)


theta2_agarr_1 = acosd(Arg_1) - 121;
theta2_agarr_2 = -acosd(Arg_1) - 121;
elseif (0<=Arg_2) && (Arg_2<=180)
theta2_agarr_1 = acosd(Arg_2) - 121;
theta2_agarr_2 = -acosd(Arg_2) - 121;
end
fprintf('La posicion de agarrotamiento 1 es = %10.4f\n',theta2_agarr_1);
fprintf('La posicion de agarrotamiento 2 es =%10.4f\n',theta2_agarr_2);

EJERCICIO2
%Ejercicio2
clc;
clear all;
for theta2=0:10:360;
a=1;
b=2.06;
c=2.33;
d=2.22;
ap=3.06;
bp=sqrt(ap^2+b^2-2*ap*b*cosd(31));
gamma=asind(b*sind(31)/bp);
k1=d/a;
k2=d/c;
k3=((a^2)-(b^2)+(c^2)+(d^2))/(2*a*c);
k4=d/b;
k5=((c^2)-(d^2)-(a^2)-(b^2))/(2*a*b);
D=cosd(theta2)-k1+k4*cosd(theta2)+k5;
E=-2*sind(theta2);
F=k1+(k4-1)*cosd(theta2)+k5;
A=cosd(theta2)-k1-k2*cosd(theta2)+k3;
B=-2*sind(theta2);
C=k1-(k2+1)*cosd(theta2)+k3;
theta42=2*atand((-B-sqrt((B^2)-4*A*C))/(2*A));
theta32=2*atand((-E-sqrt((E^2)-4*D*F))/(2*D));
alpha_a=gamma-(theta32-31);
ap_xa=ap*cosd(theta32-31);
ap_ya=ap*sind(theta32-31);
%plot([0;a*cosd(theta2)],[0;a*sind(theta2)],'o-','linewidth',2); hold on
%plot([0;d],[0;0],'o-','linewidth',2); hold on
%plot([d;d+c*cosd(theta42)],[0;c*sind(theta42)],'o-','linewidth',2); hold
on
%plot([a*cosd(theta2) a*cosd(theta2)+b*cosd(theta32)],
[a*sind(theta2);a*sind(theta2)+b*sind(theta32)],'o-','linewidth',2); hold
on
%plot([a*cosd(theta2) a*cosd(theta2)+ap_xa],[a*sind(theta2)
a*sind(theta2)+ap_ya],'o-','linewidth',2); hold on
%plot([d+c*cosd(theta42);a*cosd(theta2)+ap_xa],
[c*sind(theta42);a*sind(theta2)+ap_ya],'o-','linewidth',2); hold on
Z2=a*(cosd(theta2)+i*sind(theta2));
Z3_a=b*(cosd(theta32)+i*sind(theta32));
Z4_a=c*(cosd(theta42)+i*sind(theta42));
Z1=d;
AP=ap_xa+ap_ya*i;
pa_x=a*cosd(theta2)+ap_xa;
pa_y=a*sind(theta2)+ap_ya;
Z=[0 Z2 Z2+Z3_a Z2+Z3_a-Z4_a ];
P=[Z2 Z2+AP];B=[Z2+Z3_a Z2+AP];
subplot(4,1,1)
plot(Z,'o-','linewidth',2); hold on
plot(P,'o-','linewidth',2); hold on
plot(B,'o-','linewidth',2); hold on
plot([0 Z1],[0 0],'o-','linewidth',2); hold off
grid on;
axis ([-1.5 4 -2 5]);
pause(0.1);
subplot(4,1,2)
plot(pa_x,pa_y,'o-','linewidth',2); hold on
axis ([1.5 4 0 3])
grid on
subplot(4,1,3)
plot(theta2,Z3_a,'o-','linewidth',2); hold on
grid on
subplot(4,1,4)
plot(theta2,Z4_a,'o-','linewidth',2);hold on
grid on
end

EJERCICIO3
%ejercicio3
clc;
clear all;
syms t
a=2.17;
b=2.067;
c=2.31;
d=1;
e=5.4;
for n=0:360;
theta2=n;
theta1=0;
k1=d/a;
k2=d/c;
k3=((a^2)-(b^2)+(c^2)+(d^2))/(2*a*c);
k4=d/b;
k5=((c^2)-(d^2)-(a^2)-(b^2))/(2*a*b);
D=cosd(theta2)-k1+k4*cosd(theta2)+k5;
E=-2*sind(theta2);
F=k1+(k4-1)*cosd(theta2)+k5;
A=cosd(theta2)-k1-k2*cosd(theta2)+k3;
B=-2*sind(theta2);
C=k1-(k2+1)*cosd(theta2)+k3;
theta4=2*atand((-B-sqrt((B^2)-4*A*C))/(2*A));%abierto -
theta3=2*atand((-E-sqrt((E^2)-4*D*F))/(2*D));
% theta4=2*atand((-B+sqrt((B^2)-4*A*C))/(2*A)); %cruzado +
%theta3=2*atand((-E+sqrt((E^2)-4*D*F))/(2*D));
%theta6=102;
%y=-c^2+(c*cosd(theta4)-t*cosd(102))^2+(c*sind(theta4)-t*sind(102))^2;
%r6=double(solve(y));
%z6=r6(2);
%disp(z6);
epsilon=theta4-102;
alpha=asind(c*sind(epsilon)/e);
tau=180-theta4;
phi=alpha-tau;
theta5=180-alpha-tau-epsilon;
niu=theta4-theta5;
T=[cosd(102) sind(102);-sind(102) cosd(102)];
z1=[d*cosd(theta1);d*sind(theta1)];
z2=[a*cosd(theta2);a*sind(theta2)];
z3=[b*cosd(theta3);b*sind(theta3)];
z4=[c*cosd(theta4);c*sind(theta4)];
z5=[e*cosd(theta5);e*sind(theta5)];
%z6_v=[z6*cosd(theta6); z6*sind(theta6)];
%z1a=d*cosd(theta1)+i*d*sind(theta1);
%z2a=a*cosd(theta2)+i*a*sind(theta2);
%z3a=b*cosd(theta3)+i*b*sind(theta3);
%z4a=c*cosd(theta4)+i*c*sind(theta4);
%z5a=e*cosd(theta5)+i*e*sind(theta5);
%z6a=z6*cosd(theta6)+i*z6*sind(theta6);
L1=z1;
L2=z2;
L3=z3;
L4=z4;
L5=z5;
%L6=z6_v;
R1=T*L1;
R2=T*L2;
R3=T*L3;
R4=T*L4;
R5=T*L5;
%R6=T*L6;
subplot(4,1,1)
plot([0 R2(1)],[0 R2(2)],'o-','linewidth',2); hold on
plot([R2(1) R2(1)+R3(1)],[R2(2) R2(2)+R3(2)],'o-','linewidth',2); hold on
plot([R2(1)+R3(1) R2(1)+R3(1)-R4(1)],[R2(2)+R3(2) R2(2)+R3(2)-
R4(2)],'o-','linewidth',2); hold on
plot([R1(1) R1(1)+R4(1)+R5(1)],[R1(2)
R1(2)+R4(2)+R5(2)],'o-','linewidth',2); hold on
plot([R2(1)+R3(1) R2(1)+R3(1)+R5(1)],[R2(2)+R3(2)
R2(2)+R3(2)+R5(2)],'o-','linewidth',2); hold on
%plot([R1(1)+R6(1) R1(1)+R6(1)+R5(1)],[R1(2)+R6(2)
R1(2)+R6(2)+R5(2)],'o-','linewidth',2); hold on
plot([0 R1(1)],[0 R1(2)],'o-','linewidth',2); hold off
%plot([0 L2(1)],[0 L2(2)],'o-','linewidth',2);hold on
%plot([L2(1) L2(1)+L3(1)],[L2(2) L2(2)+L3(2)],'o-','linewidth',2);hold on
%plot([L2(1)+L3(1) L2(1)+L3(1)-L4(1)],[L2(2)+L3(2) L2(2)+L3(2)-
L4(2)],'o-','linewidth',2); hold on
%plot([L2(1)+L3(1) L5(1)],[ L2(2)+L3(2) L5(2)],'o-','linewidth',2); hold
on
%plot([L1(1) L6(1)],[L1(2) L6(2)],'o-','linewidth',2);
%plot([0 L1(1)],[0 L1(2)] ,'o-','linewidth',2); hold off
%L=[0 L2 L2+L3 L2+L3-L4];
%P=[L1 L1+L4 L1+L4-L5 ];
%subplot(4,1,1)
%plot(L,'o-','linewidth',2); hold on
%plot(P,'o-','linewidth',2); hold on
%plot(P,'linewidth',2); hold on
%plot([0 d*cosd(theta1)],[0 d*sind(theta1)],'o-','linewidth',2); hold off
axis([-6 6 -6 6]);
grid on
subplot(4,1,2)
plot(R4(1),R4(2),'o-','linewidth',2); hold on
subplot(4,1,3)
plot(R2(1)+R3(1)+R5(1),R2(2)+R3(2)+R5(2),'o-','linewidth',2); hold on
subplot(4,1,4)
plot(theta2,niu,'o-','linewidth',2); hold on
pause(0.1);
end

EJERCICIO5
%Ejercicio5
clc;
clear all;
theta2a=(0:360).';
theta4_1 = (1:360).';
theta4_2 = (1:360).';
theta3_1 = (1:360).';
theta3_2 = (1:360).';
thetaAP = (1:360).';
Px = (1:360).';
Py = (1:360).';
rango = (1:360).';

for theta2=0:360;
a=1; %R2
b=1.2; % R3
c=1.167; % R4
d=2.305;% R1
ap=1.5;
theta1=330;
k1=d/a;
k2=d/c;
k3=((a^2)-(b^2)+(c^2)+(d^2))/(2*a*c);
k4=d/b;
k5=((c^2)-(d^2)-(a^2)-(b^2))/(2*a*b);
D=cosd(theta2)-k1+k4*cosd(theta2)+k5;
E=-2*sind(theta2);
F=k1+(k4-1)*cosd(theta2)+k5;
A=cosd(theta2)-k1-k2*cosd(theta2)+k3;
B=-2*sind(theta2);
C=k1-(k2+1)*cosd(theta2)+k3;
theta4=2*atand((-B-sqrt((B^2)-4*A*C))/(2*A));
theta3=2*atand((-E-sqrt((E^2)-4*D*F))/(2*D));
ap_xa=ap*cosd(theta3+180);
ap_ya=ap*sind(theta3+180);
T=[cosd(30) sind(30); -sind(30) cosd(30)];
L1=[d*cosd(theta1);d*sind(theta1)];
L2=[a*cosd(theta2);a*sind(theta2)];
L3=[b*cosd(theta3);b*sind(theta3)];
L4=[c*cosd(theta4);c*sind(theta4)];
AP_1=[ap*cosd(theta3+180);ap*sind(theta3+180)];
R1=L1;
R2=T*L2;
R3=T*L3;
R4=T*L4;
P=T*AP_1;
OP=R2+P;
subplot(2,1,1)
plot([0 R1(1)],[0 R1(2)],'o-','linewidth',2); hold on
plot([0 R2(1)],[0 R2(2)],'o-','linewidth',2); hold on
plot([R2(1) R2(1)+R3(1)],[R2(2) R2(2)+R3(2)],'o-','linewidth',2); hold on
plot([R2(1)+R3(1) R2(1)+R3(1)-R4(1)],[R2(2)+R3(2) R2(2)+R3(2)-
R4(2)],'o-','linewidth',2); hold on
plot([R2(1) R2(1)+P(1)],[R2(2) R2(2)+P(2)],'o-','linewidth',2); hold on
% plot([0;a*cosd(theta2)],[0;a*sind(theta2)],'o-','linewidth',2); hold on
%plot([d*cosd(theta1);d*cosd(theta1)+c*cosd(theta42)],
[d*sind(theta1);c*sind(theta42)],'o-','linewidth',2); hold on
%plot([a*cosd(theta2) a*cosd(theta2)+b*cosd(theta32)],
[a*sind(theta2);a*sind(theta2)+b*sind(theta32)],'o-','linewidth',2); hold
on
%plot([a*cosd(theta2) a*cosd(theta2)+ap_xa],[a*sind(theta2)
a*sind(theta2)+ap_ya],'o-','linewidth',2); hold on
%plot([d*cosd(theta1)+c*cosd(theta42);a*cosd(theta2)+ap_xa],
[c*sind(theta43}-jk);a*sind(theta2)+ap_ya],'o-','linewidth',2); hold on
%plot([0;d*cosd(theta1)],[0;d*sind(theta1)],'o-','linewidth',2); hold off
plot([0 R1(1)],[0 R1(2)],'o-','linewidth',2); hold off
axis ([-3 5 -2 3]);
grid on
pause(0.1);
end
for n = 0:1:360
K_1 = d/a;
K_2 = d/c;
K_3 = (a^2- b^2 + c^2 + d^2 ) /(2*a*c);
K_4 = d/b;
K_5 =(c^2 - d^2 - a^2 - b^2)/(2*a*b);

%Ahora Definimos las constantes A,B,C%


A_o = cosd(theta2a) - K_1 - K_2*cosd(theta2a) + K_3;
B_o = -2*sind(theta2a);
C_o = K_1 - (K_2+1)*cosd(theta2a) + K_3;
D_o = cosd(theta2a) - K_1 + K_4*cosd(theta2a) + K_5;
E_o = -2*sind(theta2a);
F_o = K_1 + (K_4-1)*cosd(theta2a) + K_5;

%Obtenemos theta4%
theta4_1a = 2*atand((-B_o(n+1)+sqrt((B_o(n+1)^2)-
4*A_o(n+1)*C_o(n+1)))/(2*A_o(n+1)));
theta4_2a = 2*atand((-B_o(n+1)-sqrt((B_o(n+1)^2)-
4*A_o(n+1)*C_o(n+1)))/(2*A_o(n+1)));
%Obtenemos theta3%
theta3_1a = 2*atand((-E_o(n+1)+sqrt((E_o(n+1)^2)-
4*D_o(n+1)*F_o(n+1)))/(2*D_o(n+1)));
theta3_2a = 2*atand((-E_o(n+1)-sqrt((E_o(n+1)^2)-
4*D_o(n+1)*F_o(n+1)))/(2*D_o(n+1)));

if isreal(theta4_2a) && isreal(theta3_2a)


theta4_2(n+1,1) = theta4_2a;
theta3_2(n+1,1) = theta3_2a;
%theta AP con respecto al eje local%
thetaAP(n+1,1) = 180-(30-theta3_2(n+1,1));
%Coordenadas del Punto P%
Px(n+1,1) = a*cosd(theta2a(n+1,1)-30) +
1.5*cosd(thetaAP(n+1,1));
Py(n+1,1) = a*sind(theta2a(n+1,1)-30) +
1.5*sind(thetaAP(n+1,1));
else
theta4_2(n+1,1) = 0;
theta3_2(n+1,1) = 0;
%theta AP con respecto al eje local%
thetaAP(n+1,1) = 0;
%Coordenadas del Punto P%
Px(n+1,1) = 0;
Py(n+1,1) = 0;
end
if abs(Px(n+1,1)+0.5) <= 0.005
rango(n+1,1) = theta2a(n+1,1);
else
rango(n+1,1) = 0;
end

%subplot(3,1,2)
% Z2=a*(cosd(theta2)+i*sind(theta2));
%Z3=b*(cosd(theta3)+i*sind(theta3));
%Z4=c*(cosd(theta4)+i*sind(theta4));
%Z1=d;
%plot([0 a*cosd(theta2)],[0 a*sind(theta2)],'o-','linewidth',2); hold on
%plot([d d+c*cosd(theta4)],[0 c*sind(theta4)],'o-','linewidth',2); hold
on
%plot([a*cosd(theta2) a*cosd(theta2)+b*cosd(theta3)],[a*sind(theta2)
a*sind(theta2)+b*sind(theta3)],'o-','linewidth',2); hold on
%plot([a*cosd(theta2)+b*cosd(theta3) a*cosd(theta2)+b*cosd(theta3)-
c*cosd(theta4)],[a*sind(theta2)+b*sind(theta3)
a*sind(theta2)+b*sind(theta3)-c*sind(theta4)],'o-','linewidth',2); hold
on
%plot([a*cosd(theta2) a*cosd(theta2)+ap_xa],[a*sind(theta2)
a*sind(theta2)+ap_ya],'o-','linewidth',2); hold on
%plot([0 d],[0 0],'o-','linewidth',2); hold off

%Z=[0 Z2 Z2+Z3 Z2+Z3-Z4 Z2+Z3-Z4-Z1];


%P=[Z2 Z2+AP_2];
%subplot(2,1,1)
%plot(Z,'o-','linewidth',2); hold on
%plot(P,'o-','linewidth',2); hold on
%plot([0 d],[0 0],'o-','linewidth',2); hold off
%axis ([-3 5 -2 2]);
%grid on
%subplot(3,1,3)
%plot([0 P(1)],[0 P(2)],'o-','linewidth',2);
%line(real(),imag(),'Color','k','Linewidth',2);
%grid on;
%axis ([-2 2 -2 2]);
end
subplot(2,1,2)
plot(Px(:,1),Py(:,1),'o-','linewidth',2);
grid on

You might also like