clear all;
% Ingreso de datos
l1=10;
l2=8;
l3=2;
r=7;
cx=10;
cy=0;
q11=180;
q12=-180;
q21=80;
q22=-180;
q31=10;
q32=0;
% Gráfico de espacio de trabajo
hold on
figure(1)
for i=0:0.1:360
x=(l1+l2)*cosd(i);
y=(l1+l2)*sind(i);
x2=(l1-l2)*cosd(i);
y2=(l1-l2)*sind(i);
plot3(x,y,0)
plot3(x2,y2,0)
end
for i=0:0.1:360
x=(l1+l2)*cosd(i);
y=(l1+l2)*sind(i);
x2=(l1-l2)*cosd(i);
y2=(l1-l2)*sind(i);
plot3(x,y,10)
plot3(x2,y2,10)
end
for i=0:10:360
x=cx+r*cosd(i);
y=cy+r*sind(i);
plot3(x,y,0,'.r')
q3=0;
q2=-acosd((x^2+y^2-l1^2-l2^2)/(2*l1*l2));
q1=atand(y/x)+asind((sind(q2)*l2)/sqrt(x^2+y^2));
val1=l1*cosd(q1)+l2*cosd(q1-q2);
val2=l1*sind(q1)+l2*sind(q1-q2);
line([0 l1*cosd(q1)],[0 l1*sind(q1)],[0 0],'LineWidth',1);
plot3([0 l1*cosd(q1)],[0 l1*sind(q1)],[0 0],'g','LineWidth',1);
line([l1*cosd(q1) l1*cosd(q1)+l2*cosd(q1-q2)],[l1*sind(q1) l1*sind(q1)+l2*sind(q1-
q2)],[0 0],'LineWidth',1);
plot3([l1*cosd(q1) l1*cosd(q1)+l2*cosd(q1-q2)],[l1*sind(q1) l1*sind(q1)+l2*sind(q1-
q2)],[0 0],'g','LineWidth',1);
end
% Gráfico de espacio articular
figure(2)
hold on
line([q11 q11],[q21 q22],[0 0],'LineWidth',1);
plot3([q11 q11],[q21 q22],[0 0],'ro','LineWidth',1);
line([q12 q12],[q21 q22],[0 0],'LineWidth',1);
