Tarea 6

Universidad Nacional Autónoma de México

Facultad de Ingeniería
Robótica Industrial

Alumno:
Robledo Castillo Rodrigo
Profesor: Iván Alejandro Gutiérrez Giles
Grupo: 1

1. Código en Matlab para la cinemática inversa de la Tarea 6

%Programa para obtener la cinemática inversa de un robot planar

% de 3 GL con 3 ejes rotativos
% Robledo Castillo Rodrigo
%Entradas
Ox=0.3;
Oy=0.4; %Longitud
phiDegree=90; %ángulo phi medido de x0 a x3
phi=phiDegree*pi/180;

l1=0.6; %Longitud de los brazos

l2=0.6;
l3=0.5;

%Cálculo de los parámetros theta

r=sqrt((Ox-l3*cos(phi))^2 + (Oy-l3*sin(phi))^2 );
D = (r^2 - l1^2 - l2^2)/(2*l1*l2);

%Cálculo con el codo abajo para las primeras 2 articulaciones

theta2 = atan2(sqrt(1-D^2), D );
theta1 = atan2(Oy - l3*sin(phi), Ox-l3*cos(phi)) - atan2(l2*sin(theta2),l1+l2*cos(theta2));
theta3 = phi -theta1 -theta2;
[theta1*180/pi,theta2*180/pi,theta3*180/pi]

O0= [0,0];
O1= [l1*cos(theta1), l1*sin(theta1)];
O2= O1+[l2*cos(theta1+theta2),l2*sin(theta1+theta2)];
O3= O2+[l3*cos(phi),l3*sin(phi)];
Dots= [O0;O1;O2;O3];
subplot(2,1,1)
line(Dots(:,1),Dots(:,2))

%Cálculo con el codo arriba para las primeras 2 articulacione

theta2 = atan2(-sqrt(1-D^2), D );

1
theta1 = atan2(Oy - l3*sin(phi), Ox-l3*cos(phi)) - atan2(l2*sin(theta2),l1+l2*cos(theta2));
theta3 = phi -theta1 -theta2;
[theta1*180/pi,theta2*180/pi,theta3*180/pi]

O0= [0,0];
O1= [l1*cos(theta1), l1*sin(theta1)];
O2= O1+[l2*cos(theta1+theta2),l2*sin(theta1+theta2)];
O3= O2+[l3*cos(phi),l3*sin(phi)];
Dots= [O0;O1;O2;O3];
subplot(2,1,2)
line(Dots(:,1),Dots(:,2))

Haciendo pruebas con los parámetros especificados en el código se obtienen las siguientes gráficas:

Figura 1: Prueba del programa de Matlab.

Figura 2: Salidas de los ángulos en grados.