MATLAB

CINEMATICA DIRECTA CON LA MATRIZ DENAVIT HARTENBERG

Programas para controlar el robot PUMA 560

Autor:

Ingeniero Electrónico: Monteza Zevallos Fidel Tomas

Diciembre de 2016 (Primera Edición)

V. J. M. J. / A. M. M. J.
 Índice general

Introducción

1. Cinemática Directa con la matriz Denavit Hartenberg (algoritmo del archivo: DENAVIT_MATRIZ)

2. Líneas del algoritmo del archivo: PUMA560_1A

3. Líneas del algoritmo del archivo: PUMA560_1B

4. Líneas del algoritmo del archivo: PUMA560_2A

5. Líneas del algoritmo del archivo: PUMA560_3A

6. Líneas del algoritmo del archivo: PUMA560_4A

7. Líneas del algoritmo del archivo: PUMA560_4B

 Introducción
Denavit-Hartenberg propusieron en 1955 un método matricial que permite establecer de manera sistemática un sistema de coordenadas (Si) ligado a cada eslabón i de una
cadena articulada, pudiéndose determinar a continuación las ecuaciones cinemáticas de la cadena completa.

Según la representación D-H, escogiendo adecuadamente los sistemas de coordenadas asociados para cada eslabón, será posible pasar de uno al siguiente mediante 4
transformaciones básicas que dependen exclusivamente de las características geométricas del eslabón.

Estas transformaciones básicas consisten en una sucesión de rotaciones y traslaciones que permitan relacionar el sistema de referencia del elemento i con el sistema del
elemento i-1. Las transformaciones en cuestión son las siguientes:

Rotación alrededor del eje Zi-1 un ángulo i.

Traslación a lo largo de Zi-1 una distancia di; vector di ( 0,0,di ).
Traslación a lo largo de Xi una distancia ai; vector ai ( 0,0,ai ).
Rotación alrededor del eje Xi, un ángulo i.

Dado que el producto de matrices no es conmutativo, las transformaciones se han de realizar en el orden indicado. De este modo se tiene que:

i-1A i = T( z, i ) T( 0,0,di ) T ( ai,0,0 ) T( x, i )

Y realizando el producto de matrices:
Donde i, ai, di, i, son los parámetros D-H del eslabón i, basta con identificarlos para obtener matrices A y relacionar así todos y cada uno de los eslabones del robot.

Como se ha indicado, para que la matriz i-1Ai, relacione los sistemas (Si) y (Si-1), es necesario que los sistemas se hayan escogido de acuerdo a unas determinadas normas.
Estas, junto con la definición de los 4 parámetros de Denavit-Hartenberg, conforman el siguiente algoritmo para la resolución del problema cinemático directo:
El robot PUMA (Programmable Universal Machine for Assembly, o Programmable Universal Manipulation Arm) es un brazo robot industrial desarrollado por Victor Scheinman en
la empresa pionera en robótica Unimation. Inicialmente desarrollado para General Motors, el brazo robot PUMA nació de los diseños iniciales inventados por Scheinman
mientras se encontraba en el MIT y en la Stanford University.
Unimation produjo PUMA’s durante algunos años hasta que fue absorbida por Westinghouse, y posteriormente por la empresa suiza Stäubli. Nokia Robotics manufacturó cerca
1500 brazos robots PUMA durante los años 1980, siendo el PUMA-560 el modelo más popular entre los clientes. Nokia vendió su división de robótica en 1990.
En 2002 la organización General Motors Controls, Robotics and Welding (CRW) donó el prototipo original del brazo robot PUMA al Museo Nacional de Historia Americana,
reconociéndose así su importancia en el desarrollo de la robótica
1. Cinemática Directa con la matriz Denavit Hartenberg (algoritmo del archivo: DENAVIT_MATRIZ)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Para la representacion Denavit-hartenberg en Cinematica Directa se requieren 4 parámetros: a(i), alfa(i), teta(i), d(i)
% • 2 relativos a la forma y tamaño del eslabón: a(i), alfa(i)
% • 2 describen posición relativa del eslabón respecto a su predecesor*: teta(i), d(i)
% Los parámetros de forma y tamaño quedan determinados en tiempo de diseño
% Los parámetros de posición relativa varían
% • teta(i) es variable si la rotación es articular (d(i) Constante)
% • d(i) variable si la rotación es prismática (teta(i) Constante)
%
% *En notación Craig es respecto al eslabón sucesivo a(i-1), alfa(i-1), teta(i), d(i)
%
% El archivo DENAVIT_MATRIZ demuestra la funcionalidad de la Matriz de transformación homogénea del metodo DENAVIT-HARTENBERG.
%
% A partir de los parámetros de Denavit-Hartenberg (teta, d, a, alfa) se cuenta con cuatro matrices principales
% De rotacion angular teta (R_teta); de desplazamiento d (D_d); de desplazamiento a (D_a) y de rotacion angular alfa (R_alfa)
% Donde al final la matriz A sera el resultado del producto de estas cuatro matrices demostrando el metodo DENAVIT-HARTENBERG
% Revisar el archivo PDF CINEMATICA DIRECTA (Procedimiento Denavit-Hartenberg) pagina 9
%
% Igualmente se tiene la matriz B que es la representacion directa del metodo DENAVIT-HARTENBERG
%
% teta : ángulo existiría entre las líneas normales de la articulación i si se cortasen en el mismo punto del eje i
% d : distancia entre las intersecciones de las normales comunes al eje i, medida a lo largo de i
% a :(longitud eslabón) distancia entre ejes i, i+1 de las articulaciones a lo largo de la perpendicular común
% alfa :(ángulo torsión) ángulo que existiría entre ejes i,i+1 si se cortasen en punto de corte de la perpendicular común
%
% INGENIERO ELECTRONICO MONTEZA ZEVALLOS FIDEL TOMAS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear; % Limpiamos el Workspace

clc; % Limpiamos el Command Window
close all; % Cerramos todo

teta = pi/2;
% Angulo teta a girar (90°) alfa = pi/2;
% Angulo alfa a girar (90°) a=1;
% Longitud del eslabon
d=1; % Distancia entre las intersecciones

% Primera matriz de rotacion angular teta

R_teta=[cos(teta) -sin(teta) 0 0;
sin(teta) cos(teta) 0 0;
0 0 1 0;
0 0 0 1];

% Segunda matriz de desplazamiento d

D_d=[1 0 0 0;
0 1 0 0;
0 0 1 d;
0 0 0 1];

% Tercera matriz de desplazamiento a

D_a=[1 0 0 a;
0 1 0 0;
0 0 1 0;
0 0 0 1];

% Cuarta matriz de rotacion angular alfa

R_alfa=[1 0 0 0;
0 cos(alfa) -sin(alfa) 0;
0 sin(alfa) cos(alfa) 0;
0 0 0 1];

% Producto final de todas las matrices que relacionan el metodo DENAVIT-HARTENBERG

A=R_teta*D_d*D_a*R_alfa

% Matriz principal del metodo DENAVIT-HARTENBERG cuyo resultado sera igual a la matriz A
B=[cos(teta) -cos(alfa)*sin(teta) sin(alfa)*sin(teta) a*cos(teta);
sin(teta) cos(alfa)*cos(teta) -sin(alfa)*cos(teta) a*sin(teta);
0 sin(alfa) cos(alfa) d;
0 0 0 1]

Presentación en el Command Window de MATLAB de la matriz A y la matriz B mostrando idénticos resultados

2. Líneas del algoritmo del archivo: PUMA560_1A

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Archivo PUMA560_1A que se ejecuta necesariamente con la funcion ikine del Toolbox Robotic de MATLAB
% De acuerdo a la programacion el sistema se movera de la siguiente forma:
% Moverse 8 unidades en el eje Z
% Moverse 30 unidades en el eje Y
% Moverse 5 unidades en el eje X
% Moverse 15 unidades en el eje Y
% Los valores iniciales en el eje XYZ del Robot Puma son X=0.452; Y=-0.150; Z=0.432
% Los valores de variacion de cada articulacion esta de la siguiente forma:
%
% Inicio Primer Movimiento Segundo Movimiento Tercer Movimiento Cuarto Movimiento
% X=0.452 ---------------------------------------------------> 0.502
% Y=-0.15 ---------------------------> 0.15 ---------------------------------------> 0.30
% Z=0.432 -----> 0.512
%
% La variable N sera el numero de iteraciones (Velocidad de movimiento) y cada linea de programacion con el comando:
% linspace(d1,d2,N) sera las unidades de cantidad de movimiento a ser realizado
%
% Escribir en el Command Window las siguientes lineas:
% puma560
% drivebot(p560)
%
% INGENIERO ELECTRONICO MONTEZA ZEVALLOS FIDEL TOMAS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear; % Limpiamos el Workspace

clc; % Limpiamos el Command Window
close all; % Cerramos todo

puma560; % Se llama al proyecto Robot Puma creado en MATLAB

robot=p560
N=40; % La variable N es el numero de iteraciones

% Lineas de programacion para el primer tramo de movimiento de 5 unidades en el eje Z

z=linspace(0.432,0.512,N); % La variable z se distribuira desde 0.432 hasta 0.512 en N partes iguales (0.08 unidades)
x=zeros(1,N); % La variable x estara comprendida por 1 fila de N Ceros
y=x; % La variable y sera igual a lo obtenido en la variable x (1 fila de N Ceros)
for j=1:N % La variable j ira desde 1 hasta N en valores de una unidad
y(1,j)=-0.15; % Se formaran matrices y de 1 fila x (1 hasta N) columnas con los valores -0.15
x(1,j)=0.452; % Se formaran matrices x de 1 fila x (1 hasta N) columnas con los valores 0.452
end
phi=zeros(1,N); % La variable phi estara comprendida por 1 fila de N Ceros
for k=1:length(z) % La variable k ira desde 1 hasta el maximo valor de la variable z en valores de una unidad
phik=phi(k);
T(:,:,k)=[cos(phik) -sin(phik) 0 x(k); % La matriz T es la matriz homogenea
sin(phik) cos(phik) 0 y(k);
0 0 1 z(k);
0 0 0 1];
end
qzz=ikine(robot,T) % ikine es una funcion de Cinematica Inversa del Toolbox de Robotica
plot(robot,qzz)

% Lineas de programacion para el segundo tramo de movimiento de 30 unidades en el eje Y

y=linspace(-0.15,0.15,N); % La variable y se distribuira desde -0.15 hasta 0.15 en N partes iguales (0.30 unidades)
x=zeros(1,N); % La variable x estara comprendida por 1 fila de N Ceros
z=x; % La variable z sera igual a lo obtenido en la variable x (1 fila de N Ceros)
for j=1:N % La variable j ira desde 1 hasta N en valores de una unidad
x(1,j)=0.452; % Se formaran matrices x de 1 fila x (1 hasta N) columnas con los valores 0.452
z(1,j)=0.512; % Se formaran matrices z de 1 fila x (1 hasta N) columnas con los valores 0.512
end
phi=zeros(1,N);
for k=1:length(y) % La variable k ira desde 1 hasta el maximo valor de la variable y en valores de una unidad
phik=phi(k);
T(:,:,k)=[cos(phik) -sin(phik) 0 x(k); % La matriz T es la matriz homogenea
sin(phik) cos(phik) 0 y(k);
0 0 1 z(k);
0 0 0 1];
end
qyy=ikine(robot,T) % ikine es una funcion de Cinematica Inversa del Toolbox de Robotica
plot(robot,qyy)

% Lineas de programacion para el tercer tramo de movimiento de 5 unidades en el eje X

x=linspace(0.452,0.502,N); % La variable x se distribuira desde 0.452 hasta 0.502 en N partes iguales (0.05 unidades)
y=zeros(1,N); % La variable y estara comprendida por 1 fila de N Ceros
z=y; % La variable z sera igual a lo obtenido en la variable y (1 fila de N Ceros)
for j=1:N % La variable j ira desde 1 hasta N en valores de una unidad
y(1,j)=0.15; % Se formaran matrices y de 1 fila x (1 hasta N) columnas con los valores 0.15
z(1,j)=0.512; % Se formaran matrices z de 1 fila x (1 hasta N) columnas con los valores 0.512
end
phi=zeros(1,N); % La variable phi estara comprendida por 1 fila de N Ceros
for k=1:length(x) % La variable k ira desde 1 hasta el maximo valor de la variable x en valores de una unidad
phik=phi(k);
T(:,:,k)=[cos(phik) -sin(phik) 0 x(k); % La matriz T es la matriz homogenea
sin(phik) cos(phik) 0 y(k);
0 0 1 z(k);
0 0 0 1];
end
qxx=ikine(robot,T) % ikine es una funcion de Cinematica Inversa del Toolbox de Robotica
plot(robot,qxx)

% Lineas de programacion para el cuarto tramo de movimiento de 15 unidades en el eje Y

y=linspace(0.15,0.30,N); % La variable y se distribuira desde 0.15 hasta 0.30 en N partes iguales (0.15 unidades)
x=zeros(1,N); % La variable x estara comprendida por 1 fila de N Ceros
z=x; % La variable z sera igual a lo obtenido en la variable x (1 fila de N Ceros)
for j=1:N % La variable j ira desde 1 hasta N en valores de una unidad
x(1,j)=0.502; % Se formaran matrices x de 1 fila x (1 hasta N) columnas con los valores 0.502
z(1,j)=0.512; % Se formaran matrices z de 1 fila x (1 hasta N) columnas con los valores 0.512
end
phi=zeros(1,N); % La variable phi estara comprendida por 1 fila de N Ceros
for k=1:length(y) % La variable k ira desde 1 hasta el maximo valor de la variable y en valores de una unidad
phik=phi(k);
T(:,:,k)=[cos(phik) -sin(phik) 0 x(k); % La matriz T es la matriz homogenea
sin(phik) cos(phik) 0 y(k);
0 0 1 z(k);
0 0 0 1];
end
qyy=ikine(robot,T) % ikine es una funcion de Cinematica Inversa del Toolbox de Robotica
plot(robot,qyy)
3. Líneas del algoritmo del archivo: PUMA560_1B

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Archivo PUMA560_1B que se ejecuta necesariamente con la funcion ikine del Toolbox Robotic de MATLAB
% De acuerdo a la programacion el sistema se movera de la siguiente forma:
% Moverse 150 unidades en el eje X
% Moverse 30 unidades en el eje Y
% Moverse 5 unidades en el eje Z
% Moverse 150 unidades en el eje X
% Moverse 15 unidades en el eje Y
% Los valores iniciales en el eje XYZ del Robot Puma son X=0.452; Y=-0.150; Z=0.432
% Los valores de variacion de cada articulacion esta de la siguiente forma:
%
% Inicio Primer Movimiento Segundo Movimiento Tercer Movimiento Cuarto Movimiento Quinto Movimiento
% X=0.452 -----> 0.602 ------------------------------------------------------------> 0.452
% Y=-0.15 ---------------------------> 0.15 ------------------------------------------------------------> 0.30
% Z=0.432 --------------------------------------------------> 0.482
%
% La variable N sera el numero de iteraciones (Velocidad de movimiento) y cada linea de programacion con el comando:
% linspace(d1,d2,N) sera las unidades de cantidad de movimiento a ser realizado
%
% Escribir en el Command Window las siguientes lineas:
% puma560
% drivebot(p560)
%
% INGENIERO ELECTRONICO MONTEZA ZEVALLOS FIDEL TOMAS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear; % Limpiamos el Workspace

clc; % Limpiamos el Command Window
close all; % Cerramos todo

for j=1:N % La variable j ira desde 1 hasta N en valores de una unidad

x(1,j)=0.602; % Se formaran matrices x de 1 fila x (1 hasta N) columnas con los valores 0.602
y(1,j)=0.15; % Se formaran matrices y de 1 fila x (1 hasta N) columnas con los valores 0.15
end
phi=zeros(1,N); % La variable phi estara comprendida por 1 fila de N Ceros
for k=1:length(z) % La variable k ira desde 1 hasta el maximo valor de la variable z en valores de una unidad
phik=phi(k);
T(:,:,k)=[cos(phik) -sin(phik) 0 x(k); % La matriz T es la matriz homogenea
sin(phik) cos(phik) 0 y(k);
0 0 1 z(k);
end 0 0 0 1];
puma560; % Se llama al proyecto Robot Puma creado en MATLAB
robot=p560
N=40; % La variable N es el numero de iteraciones

% Lineas de programacion para el primer tramo de movimiento de 150 unidades en el eje X

x=linspace(0.452,0.602,N); % La variable x se distribuira desde 0.452 hasta 0.602 en N partes iguales (0.15 unidades)
y=zeros(1,N); % La variable y estara comprendida por 1 fila de N Ceros
z=y; % La variable z sera igual a lo obtenido en la variable y (1 fila de N Ceros)
for j=1:N % La variable j ira desde 1 hasta N en valores de una unidad
z(1,j)=0.432; % Se formaran matrices z de 1 fila x (1 hasta N) columnas con los valores 0.432
y(1,j)=-0.15; % Se formaran matrices y de 1 fila x (1 hasta N) columnas con los valores -0.15

for j=1:N % La variable j ira desde 1 hasta N en valores de una unidad

x(1,j)=0.602; % Se formaran matrices x de 1 fila x (1 hasta N) columnas con los valores 0.602
y(1,j)=0.15; % Se formaran matrices y de 1 fila x (1 hasta N) columnas con los valores 0.15
end
phi=zeros(1,N); % La variable phi estara comprendida por 1 fila de N Ceros
for k=1:length(z) % La variable k ira desde 1 hasta el maximo valor de la variable z en valores de una unidad
phik=phi(k);
T(:,:,k)=[cos(phik) -sin(phik) 0 x(k); % La matriz T es la matriz homogenea
sin(phik) cos(phik) 0 y(k);
0 0 1 z(k);
end 0 0 0 1];
phi=zeros(1,N); % La variable phi estara comprendida por 1 fila de N Ceros
for k=1:length(x) % La variable k ira desde 1 hasta el maximo valor de la variable x en valores de una unidad
phik=phi(k);
T(:,:,k)=[cos(phik) -sin(phik) 0 x(k); % La matriz T es la matriz homogenea
sin(phik) cos(phik) 0 y(k);
0 0 1 z(k);
0 0 0 1];
end
qxx=ikine(robot,T) % ikine es una funcion de Cinematica Inversa del Toolbox de Robotica
plot(robot,qxx)

% Lineas de programacion para el segundo tramo de movimiento de 30 unidades en el eje Y

y=linspace(-0.15,0.15,N); % La variable y se distribuira desde -0.15 hasta 0.15 en N partes iguales (0.30 unidades)
x=zeros(1,N); % La variable x estara comprendida por 1 fila de N Ceros
z=x; % La variable z sera igual a lo obtenido en la variable x (1 fila de N Ceros)
for j=1:N % La variable j ira desde 1 hasta N en valores de una unidad
z(1,j)=0.432; % Se formaran matrices z de 1 fila x (1 hasta N) columnas con los valores 0.432
x(1,j)=0.602; % Se formaran matrices x de 1 fila x (1 hasta N) columnas con los valores 0.602
end
phi=zeros(1,N);
for k=1:length(y) % La variable k ira desde 1 hasta el maximo valor de la variable y en valores de una unidad
phik=phi(k);
T(:,:,k)=[cos(phik) -sin(phik) 0 x(k); % La matriz T es la matriz homogenea
sin(phik) cos(phik) 0 y(k);
0 0 1 z(k);
0 0 0 1];
end
qyy=ikine(robot,T) % ikine es una funcion de Cinematica Inversa del Toolbox de Robotica
plot(robot,qyy)

% Lineas de programacion para el tercer tramo de movimiento de 5 unidades en el eje Z

z=linspace(0.432,0.482,N); % La variable z se distribuira desde 0.432 hasta 0.482 en N partes iguales (0.05 unidades)
y=zeros(1,N); % La variable y estara comprendida por 1 fila de N Ceros

for j=1:N % La variable j ira desde 1 hasta N en valores de una unidad

x(1,j)=0.602; % Se formaran matrices x de 1 fila x (1 hasta N) columnas con los valores 0.602
y(1,j)=0.15; % Se formaran matrices y de 1 fila x (1 hasta N) columnas con los valores 0.15
end
phi=zeros(1,N); % La variable phi estara comprendida por 1 fila de N Ceros
for k=1:length(z) % La variable k ira desde 1 hasta el maximo valor de la variable z en valores de una unidad
phik=phi(k);
T(:,:,k)=[cos(phik) -sin(phik) 0 x(k); % La matriz T es la matriz homogenea
sin(phik) cos(phik) 0 y(k);
0 0 1 z(k);
end 0 0 0 1];
x=y; % La variable x sera igual a lo obtenido en la variable y (1 fila de N Ceros)

for j=1:N % La variable j ira desde 1 hasta N en valores de una unidad

x(1,j)=0.602; % Se formaran matrices x de 1 fila x (1 hasta N) columnas con los valores 0.602
y(1,j)=0.15; % Se formaran matrices y de 1 fila x (1 hasta N) columnas con los valores 0.15
end
phi=zeros(1,N); % La variable phi estara comprendida por 1 fila de N Ceros
for k=1:length(z) % La variable k ira desde 1 hasta el maximo valor de la variable z en valores de una unidad
phik=phi(k);
T(:,:,k)=[cos(phik) -sin(phik) 0 x(k); % La matriz T es la matriz homogenea
sin(phik) cos(phik) 0 y(k);
0 0 1 z(k);
end 0 0 0 1];
end
qzz=ikine(robot,T) % ikine es una funcion de Cinematica Inversa del Toolbox de Robotica
plot(robot,qzz)

% Lineas de programacion para el cuarto tramo de movimiento de 150 unidades en el eje X

x=linspace(0.602,0.452,N); % La variable x se distribuira desde 0.592 hasta 0.452 en N partes iguales (0.140 unidades)
y=zeros(1,N); % La variable y estara comprendida por 1 fila de N Ceros
z=x; % La variable z sera igual a lo obtenido en la variable y (1 fila de N Ceros)
for j=1:N % La variable j ira desde 1 hasta N en valores de una unidad
z(1,j)=0.482; % Se formaran matrices x de 1 fila x (1 hasta N) columnas con los valores 0.482
y(1,j)=0.15; % Se formaran matrices z de 1 fila x (1 hasta N) columnas con los valores 0.15
end
phi=zeros(1,N); % La variable phi estara comprendida por 1 fila de N Ceros
for k=1:length(x) % La variable k ira desde 1 hasta el maximo valor de la variable x en valores de una unidad
phik=phi(k);
T(:,:,k)=[cos(phik) -sin(phik) 0 x(k); % La matriz T es la matriz homogenea
sin(phik) cos(phik) 0 y(k);
0 0 1 z(k);
0 0 0 1];
end
qxx=ikine(robot,T) % ikine es una funcion de Cinematica Inversa del Toolbox de Robotica
plot(robot,qxx)

% Lineas de programacion para el quinto tramo de movimiento de 15 unidades en el eje Y

y=linspace(0.15,0.30,N); % La variable y se distribuira desde 0.15 hasta 0.30 en N partes iguales (0.15 unidades)
x=zeros(1,N); % La variable x estara comprendida por 1 fila de N Ceros
z=x; % La variable z sera igual a lo obtenido en la variable x (1 fila de N Ceros)
for j=1:N % La variable j ira desde 1 hasta N en valores de una unidad
z(1,j)=0.482; % Se formaran matrices z de 1 fila x (1 hasta N) columnas con los valores 0.432
x(1,j)=0.452; % Se formaran matrices x de 1 fila x (1 hasta N) columnas con los valores 0.602
end
phi=zeros(1,N);
for k=1:length(y) % La variable k ira desde 1 hasta el maximo valor de la variable y en valores de una unidad
phik=phi(k);
T(:,:,k)=[cos(phik) -sin(phik) 0 x(k); % La matriz T es la matriz homogenea
sin(phik) cos(phik) 0 y(k);
0 0 1 z(k);
0 0 0 1];
end
qyy=ikine(robot,T) % ikine es una funcion de Cinematica Inversa del Toolbox de Robotica
plot(robot,qyy)
4. Líneas del algoritmo del archivo: PUMA560_2A

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Archivo PUMA560_2A que se ejecuta necesariamente con la funcion ikine del Toolbox Robotic de MATLAB
% De acuerdo a la programacion el sistema se movera de la siguiente forma:
% Moverse 8 unidades en el eje Z
% Moverse 30 unidades en el eje Y
% Moverse 5 unidades en el eje X
% Moverse 15 unidades en el eje Y
% Los valores iniciales en el eje XYZ del Robot Puma son X=0.452; Y=-0.150; Z=0.432
% Los valores de variacion de cada articulacion esta de la siguiente forma:
%
% Inicio Primer Movimiento Segundo Movimiento Tercer Movimiento Cuarto Movimiento
% X=0.452 ---------------------------------------------------> 0.502
% Y=-0.15 ---------------------------> 0.15 ---------------------------------------> 0.30
% Z=0.432 -----> 0.512
%
% Las variables N seran el numero de iteraciones (Velocidad de movimiento) y cada linea de programacion con el comando:
% linspace(d1,d2,N) seran las unidades de cantidad de movimiento a ser realizado
%
% Escribir en el Command Window las siguientes lineas:
% puma560
% drivebot(p560)
%
% INGENIERO ELECTRONICO MONTEZA ZEVALLOS FIDEL TOMAS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear; % Limpiamos el Workspace

clc; % Limpiamos el Command Window
close all; % Cerramos todo

puma560; % Se llama al proyecto Robot Puma creado en MATLAB

robot=p560
N1=40; % La variable N1 es
el numero de iteraciones para el eje Z N2=70;
% La variable N2 es el numero de iteraciones para el eje Y N3=90;
% La variable N3 es el numero de iteraciones para el eje X N4=120;
% La variable N4 es el numero de iteraciones para el eje Y

% Lineas de programacion para el primer tramo de movimiento de N1 iteraciones en el eje Z

z=linspace(0.432,0.512,N1); % La variable z se distribuira desde 0.432 hasta 0.512 en N1 partes iguales (0.08 unidades)
x=zeros(1,N1); % La variable x estara comprendida por 1 fila de N1 Ceros
y=x; % La variable y sera igual a lo obtenido en la variable x (1 fila de N1 Ceros)
for j=1:N1 % La variable j ira desde 1 hasta N1 en valores de una unidad
y(1,j)=-0.15; % Se formaran matrices y de 1 fila x (1 hasta N1) columnas con los valores -0.15
 x(1,j)=0.452; % Se formaran matrices x de 1 fila x (1 hasta N1) columnas con los valores 0.452
end
phi=zeros(1,N1); % La variable phi estara comprendida por 1 fila de N1 Ceros
for k=1:length(z) % La variable k ira desde 1 hasta el maximo valor de la variable z en valores de una unidad
phik=phi(k);
T(:,:,k)=[cos(phik) -sin(phik) 0 x(k); % La matriz T es la matriz homogenea
sin(phik) cos(phik) 0 y(k);
0 0 1 z(k);
0 0 0 1];
end
qzz=ikine(robot,T) % ikine es una funcion de Cinematica Inversa del Toolbox de Robotica
plot(robot,qzz)

% Lineas de programacion para el segundo tramo de movimiento de N2 iteraciones en el eje Y

y=linspace(-0.15,0.15,N2); % La variable y se distribuira desde -0.15 hasta 0.15 en N2 partes iguales (0.30 unidades)
x=zeros(1,N2); % La variable x estara comprendida por 1 fila de N2 Ceros
z=x; % La variable z sera igual a lo obtenido en la variable x (1 fila de N2 Ceros)
for j=1:N2 % La variable j ira desde 1 hasta N2 en valores de una unidad
x(1,j)=0.452; % Se formaran matrices x de 1 fila x (1 hasta N2) columnas con los valores 0.452
z(1,j)=0.512; % Se formaran matrices z de 1 fila x (1 hasta N2) columnas con los valores 0.512
end
phi=zeros(1,N2);
for k=1:length(y) % La variable k ira desde 1 hasta el maximo valor de la variable y en valores de una unidad
phik=phi(k);
T(:,:,k)=[cos(phik) -sin(phik) 0 x(k); % La matriz T es la matriz homogenea
sin(phik) cos(phik) 0 y(k);
0 0 1 z(k);
0 0 0 1];
end
qyy=ikine(robot,T) % ikine es una funcion de Cinematica Inversa del Toolbox de Robotica
plot(robot,qyy)

% Lineas de programacion para el tercer tramo de movimiento de N3 iteraciones en el eje X

x=linspace(0.452,0.502,N3); % La variable x se distribuira desde 0.452 hasta 0.502 en N3 partes iguales (0.05 unidades)
y=zeros(1,N3); % La variable y estara comprendida por 1 fila de N3 Ceros
z=y; % La variable z sera igual a lo obtenido en la variable y (1 fila de N3 Ceros)
for j=1:N3 % La variable j ira desde 1 hasta N3 en valores de una unidad
y(1,j)=0.15; % Se formaran matrices y de 1 fila x (1 hasta N3) columnas con los valores 0.15
z(1,j)=0.512; % Se formaran matrices z de 1 fila x (1 hasta N3) columnas con los valores 0.512
end
phi=zeros(1,N3); % La variable phi estara comprendida por 1 fila de N3 Ceros
for k=1:length(x) % La variable k ira desde 1 hasta el maximo valor de la variable x en valores de una unidad
phik=phi(k);
T(:,:,k)=[cos(phik) -sin(phik) 0 x(k); % La matriz T es la matriz homogenea
sin(phik) cos(phik) 0 y(k);
 0 0 1 z(k);
0 0 0 1];
end
qxx=ikine(robot,T) % ikine es una funcion de Cinematica Inversa del Toolbox de Robotica
plot(robot,qxx)

% Lineas de programacion para el cuarto tramo de movimiento de N4 iteraciones en el eje Y

y=linspace(0.15,0.30,N4); % La variable y se distribuira desde 0.15 hasta 0.30 en N4 partes iguales (0.30 unidades)
x=zeros(1,N4); % La variable x estara comprendida por 1 fila de N4 Ceros
z=x; % La variable z sera igual a lo obtenido en la variable x (1 fila de N Ceros)
for j=1:N4 % La variable j ira desde 1 hasta N4 en valores de una unidad
x(1,j)=0.502; % Se formaran matrices x de 1 fila x (1 hasta N4) columnas con los valores 0.502
z(1,j)=0.512; % Se formaran matrices z de 1 fila x (1 hasta N4) columnas con los valores 0.512
end
phi=zeros(1,N4); % La variable phi estara comprendida por 1 fila de N4 Ceros
for k=1:length(y) % La variable k ira desde 1 hasta el maximo valor de la variable y en valores de una unidad
phik=phi(k);
T(:,:,k)=[cos(phik) -sin(phik) 0 x(k); % La matriz T es la matriz homogenea
sin(phik) cos(phik) 0 y(k);
0 0 1 z(k);
0 0 0 1];
end
qyy=ikine(robot,T) % ikine es una funcion de Cinematica Inversa del Toolbox de Robotica
plot(robot,qyy)
5. Líneas del algoritmo del archivo: PUMA560_3A

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Archivo PUMA560_3A que se ejecuta necesariamente con la funcion ikine del Toolbox Robotic de MATLAB
% De acuerdo a la programacion el sistema se movera de la siguiente forma:
% Moverse 8 unidades en el eje Z
% Moverse 30 unidades en el eje Y
% Moverse 5 unidades en el eje X
% Moverse 15 unidades en el eje Y
%
% Los valores iniciales en el eje XYZ del Robot Puma son X=0.452; Y=-0.150; Z=0.432
% Los valores de variacion de cada articulacion esta de la siguiente forma:
%
% Inicio Primer Movimiento Segundo Movimiento Tercer Movimiento Cuarto Movimiento
% X=0.452 ---------------------------------------------------> 0.502
% Y=-0.15 ---------------------------> 0.15 ---------------------------------------> 0.30
% Z=0.432 -----> 0.512
%
% Las variables N seran el numero de iteraciones (Velocidad de movimiento) y cada linea de programacion con el comando:
% linspace(d1,d2,N) seran las unidades de cantidad de movimiento a ser realizado
% Las variables d seran los puntos iniciales y finales de cada movimiento a ser realizado
%
% Escribir en el Command Window las siguientes lineas:
% puma560
% drivebot(p560)
%
% INGENIERO ELECTRONICO MONTEZA ZEVALLOS FIDEL TOMAS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear; % Limpiamos el Workspace

clc; % Limpiamos el Command Window
close all; % Cerramos todo

puma560; % Se llama al proyecto Robot Puma creado en MATLAB

robot=p560
N1=40; % La variable N1 es el
numero de iteraciones para el eje Z N2=70;
% La variable N2 es el numero de iteraciones para el eje Y N3=90;
% La variable N3 es el numero de iteraciones para el eje X N4=120;
% La variable N4 es el numero de iteraciones para el eje Y

px=0.452; % Valor
inicial en el eje X del Robot Puma py=-0.150;
% Valor inicial en el eje Y del Robot Puma pz=0.432;
% Valor inicial en el eje Z del Robot Puma
d1=0.432; % Punto inicial del primer movimiento del brazo robotico, puede ser 0.432
d2=0.512; % Punto final del primer movimiento del brazo robotico, puede ser 0.512
d3=-0.15; % Punto inicial del segundo movimiento del brazo robotico, puede ser -0.15
d4=0.15; % Punto final del segundo movimiento del brazo robotico, puede ser 0.15
d5=0.452; % Punto inicial del tercer movimiento del brazo robotico, puede ser 0.452
d6=0.502; % Punto final del tercer movimiento del brazo robotico, puede ser 0.502
d7=0.15; % Punto inicial del cuarto movimiento del brazo robotico, puede ser 0.15
d8=0.30; % Punto final del cuarto movimiento del brazo robotico, puede ser 0.30

% Lineas de programacion para el primer tramo de movimiento de N1 iteraciones en el eje Z

z=linspace(d1,d2,N1); % La variable z se distribuira desde 0.432 hasta 0.512 en N1 partes iguales (d2-d1=0.08 unidades)
x=zeros(1,N1); % La variable x estara comprendida por 1 fila de N1 Ceros
y=x; % La variable y sera igual a lo obtenido en la variable x (1 fila de N1 Ceros)
for j=1:N1 % La variable j ira desde 1 hasta N1 en valores de una unidad
y(1,j)=py; % Se formaran matrices y de 1 fila x (1 hasta N1) columnas con el valor inicial -0.15
x(1,j)=px; % Se formaran matrices x de 1 fila x (1 hasta N1) columnas con el valor inicial 0.452
end
phi=zeros(1,N1); % La variable phi estara comprendida por 1 fila de N1 Ceros
for k=1:length(z) % La variable k ira desde 1 hasta el maximo valor de la variable z en valores de una unidad
phik=phi(k);
T(:,:,k)=[cos(phik) -sin(phik) 0 x(k); % La matriz T es la matriz homogenea
sin(phik) cos(phik) 0 y(k);
0 0 1 z(k);
0 0 0 1];
end
qzz=ikine(robot,T) % ikine es una funcion de Cinematica Inversa del Toolbox de Robotica
plot(robot,qzz)

% Lineas de programacion para el segundo tramo de movimiento de N2 iteraciones en el eje Y

y=linspace(d3,d4,N2); % La variable y se distribuira desde -0.15 hasta 0.15 en N2 partes iguales (d4-d3=0.30 unidades)
x=zeros(1,N2); % La variable x estara comprendida por 1 fila de N2 Ceros
z=x; % La variable z sera igual a lo obtenido en la variable x (1 fila de N2 Ceros)
for j=1:N2 % La variable j ira desde 1 hasta N2 en valores de una unidad
x(1,j)=px; % Se formaran matrices x de 1 fila x (1 hasta N2) columnas con los valores 0.452 (X no ha sufrido cambios)
z(1,j)=pz+(d2-d1);% Se formara matrices z de 1 fila x (1 hasta N2) columnas con valores 0.512 (Punto final movimiento anterior)
end
phi=zeros(1,N2);
for k=1:length(y) % La variable k ira desde 1 hasta el maximo valor de la variable y en valores de una unidad
phik=phi(k);
T(:,:,k)=[cos(phik) -sin(phik) 0 x(k); % La matriz T es la matriz homogenea
sin(phik) cos(phik) 0 y(k);
0 0 1 z(k);
0 0 0 1];
end
qyy=ikine(robot,T) % ikine es una funcion de Cinematica Inversa del Toolbox de Robotica
plot(robot,qyy)

% Lineas de programacion para el tercer tramo de movimiento de N3 iteraciones en el eje X

x=linspace(d5,d6,N3); % La variable x se distribuira desde 0.452 hasta 0.502 en N3 partes iguales (d6-d5=0.05 unidades)
y=zeros(1,N3); % La variable y estara comprendida por 1 fila de N3 Ceros
z=y; % La variable z sera igual a lo obtenido en la variable y (1 fila de N3 Ceros)
for j=1:N3 % La variable j ira desde 1 hasta N3 en valores de una unidad
y(1,j)=py+(d4-d3); % Se formara matrices y de 1 fila x (1 hasta N3) columnas con valores 0.15 (Y=py+(d4-d3)=-0.15+0.30=0.15)
z(1,j)=pz+(d2-d1); % Se formaran matrices z de 1 fila x (1 hasta N3) columnas con los valores 0.482 (Z no ha sufrido cambios)
end
phi=zeros(1,N3); % La variable phi estara comprendida por 1 fila de N3 Ceros
for k=1:length(x) % La variable k ira desde 1 hasta el maximo valor de la variable x en valores de una unidad
phik=phi(k);
T(:,:,k)=[cos(phik) -sin(phik) 0 x(k); % La matriz T es la matriz homogenea
sin(phik) cos(phik) 0 y(k);
0 0 1 z(k);
0 0 0 1];
end
qxx=ikine(robot,T) % ikine es una funcion de Cinematica Inversa del Toolbox de Robotica
plot(robot,qxx)

% Lineas de programacion para el cuarto tramo de movimiento de N4 iteraciones en el eje Y

y=linspace(d7,d8,N4); % La variable y se distribuira desde 0.05 hasta -0.05 en N4 partes iguales (d8-d7=0.10 unidades)
x=zeros(1,N4); % La variable x estara comprendida por 1 fila de N4 Ceros
z=x; % La variable z sera igual a lo obtenido en la variable x (1 fila de N Ceros)
for j=1:N4 % La variable j ira desde 1 hasta N4 en valores de una unidad
x(1,j)=px+(d6-d5);% Se formara matrices x de 1 fila x (1 hasta N4) columnas con valores 0.702 (X=px+(d6-d5)=0.452+0.25=0.702)
z(1,j)=pz+(d2-d1);% Se formaran matrices z de 1 fila x (1 hasta N4) columnas con los valores 0.482 (Z no ha sufrido cambios)
end
phi=zeros(1,N4); % La variable phi estara comprendida por 1 fila de N4 Ceros
for k=1:length(y) % La variable k ira desde 1 hasta el maximo valor de la variable y en valores de una unidad
phik=phi(k);
T(:,:,k)=[cos(phik) -sin(phik) 0 x(k); % La matriz T es la matriz homogenea
sin(phik) cos(phik) 0 y(k);
0 0 1 z(k);
0 0 0 1];
end
qyy=ikine(robot,T) % ikine es una funcion de Cinematica Inversa del Toolbox de Robotica
plot(robot,qyy)
6. Líneas del algoritmo del archivo: PUMA560_4A

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Archivo PUMA560_4 que se ejecuta necesariamente con la funcion ikine del Toolbox Robotic de MATLAB
% De acuerdo a la programacion el sistema se movera de la siguiente forma:
% Moverse 8 unidades en el eje Z
% Moverse 30 unidades en el eje Y
% Moverse 5 unidades en el eje X
% Moverse 15 unidades en el eje Y
%
% Los valores iniciales en el eje XYZ del Robot Puma son X=0.452; Y=-0.150; Z=0.432
% Los valores de variacion de cada articulacion esta de la siguiente forma:
%
% Inicio Primer Movimiento Segundo Movimiento Tercer Movimiento Cuarto Movimiento
% X=0.452 ---------------------------------------------------> 0.502
% Y=-0.15 ---------------------------> 0.15 ---------------------------------------> 0.30
% Z=0.432 -----> 0.512
%
% Las variables N seran el numero de iteraciones (Velocidad de movimiento) y cada linea de programacion con el comando:
% linspace(d1,d2,N) seran las unidades de cantidad de movimiento a ser realizado
% Las variables d seran los puntos iniciales y finales de cada movimiento a ser realizado
% Las variables N y las variables d podran ser ingresadas individualmente desde el Command Window
%
% Escribir en el Command Window las siguientes lineas:
% puma560
% drivebot(p560)
%
% INGENIERO ELECTRONICO MONTEZA ZEVALLOS FIDEL TOMAS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear; % Limpiamos el Workspace

clc; % Limpiamos el Command Window
close all; % Cerramos todo

puma560; % Se llama al proyecto Robot Puma creado en MATLAB

robot=p560

fprintf('\n CINEMATICA INVERSA DEL ROBOT PUMA 560 EN MATLAB') % Con (\n) se imprime en la siguiente linea
fprintf('\n ') % Con (\n) se imprime en la siguiente linea
fprintf('\n INGRESE LOS DATOS ESTABLECIDOS PARA GENERAR EL MOVIMIENTO') % Con (\n) se imprime en la siguiente linea
fprintf('\n ') % Con (\n) se imprime en la siguiente linea

fprintf('\n El valor de N1 (Numero de iteraciones del Primer Movimiento podria ser 40)') % (\n) se imprime en la siguiente linea
N1=input('\n INGRESE N1 = '); % La variable N1 es el numero de iteraciones para el eje Z
fprintf('\n El valor de N2 (Numero de iteraciones del Segundo Movimiento podria ser 70)') % (\n) se imprime en la siguiente linea
N2=input('\n INGRESE N2 = '); % La variable N2 es el numero de iteraciones para el eje Y
fprintf('\n El valor de N3 (Numero de iteraciones del Tercer Movimiento podria ser 90)') % (\n) se imprime en la siguiente linea
N3=input('\n INGRESE N3 = '); % La variable N3 es el numero de iteraciones para el eje X
fprintf('\n El valor de N4 (Numero de iteraciones del Cuarto Movimiento podria ser 120)') % (\n) se imprime en la siguiente linea
N4=input('\n INGRESE N4 = '); % La variable N4 es el numero de iteraciones para el eje Y
fprintf('\n ') % Con (\n) se imprime en la siguiente linea

fprintf('\n El valor de px (Valor inicial en el eje X del Robot Puma es 0.452)') % Con (\n) se imprime en la siguiente linea
px=input('\n INGRESE px = '); % La variable px es el Valor inicial en el eje X del Robot Puma
fprintf('\n El valor de py (Valor inicial en el eje Y del Robot Puma es -0.150)') % Con (\n) se imprime en la siguiente linea
py=input('\n INGRESE py = '); % La variable py es el Valor inicial en el eje Y del Robot Puma
fprintf('\n El valor de pz (Valor inicial en el eje Z del Robot Puma es 0.432)') % Con (\n) se imprime en la siguiente linea
pz=input('\n INGRESE pz = '); % La variable pz es el Valor inicial en el eje Z del Robot Puma
fprintf('\n ') % Con (\n) se imprime en la siguiente linea

fprintf('\n El valor de d1 (Punto inicial del primer movimiento del brazo robotico, puede ser 0.432)')
d1=input('\n INGRESE d1 = '); % Punto inicial del primer movimiento
fprintf('\n El valor de d2 (Punto final del segundo movimiento del brazo robotico, puede ser 0.512)')
d2=input('\n INGRESE d2 = '); % Punto final del primer movimiento
fprintf('\n El valor de d3 (Punto inicial del segundo movimiento del brazo robotico, puede ser -0.15)')
d3=input('\n INGRESE d3 = '); % Punto inicial del segundo movimiento
fprintf('\n El valor de d4 (Punto final del segundo movimiento del brazo robotico, puede ser 0.15)')
d4=input('\n INGRESE d4 = '); % Punto final del segundo movimiento
fprintf('\n El valor de d5 (Punto inicial del tercer movimiento del brazo robotico, puede ser 0.452)')
d5=input('\n INGRESE d5 = '); % Punto inicial del tercer movimiento
fprintf('\n El valor de d6 (Punto final del tercer movimiento del brazo robotico, puede ser 0.502)')
d6=input('\n INGRESE d6 = '); % Punto final del tercer movimiento
fprintf('\n El valor de d7 (Punto inicial del cuarto movimiento del brazo robotico, puede ser 0.15)')
d7=input('\n INGRESE d7 = '); % Punto inicial del cuarto movimiento
fprintf('\n El valor de d8 (Punto final del cuarto movimiento del brazo robotico, puede ser 0.30)')
d8=input('\n INGRESE d8 = '); % Punto final del cuarto movimiento

% Lineas de programacion para el primer tramo de movimiento de N1 iteraciones en el eje Z

z=linspace(d1,d2,N1); % La variable z se distribuira desde 0.432 hasta 0.502 en N1 partes iguales (d2-d1=0.08 unidades)
x=zeros(1,N1); % La variable x estara comprendida por 1 fila de N1 Ceros
y=x; % La variable y sera igual a lo obtenido en la variable x (1 fila de N1 Ceros)
for j=1:N1 % La variable j ira desde 1 hasta N1 en valores de una unidad
y(1,j)=py; % Se formaran matrices y de 1 fila x (1 hasta N1) columnas con el valor inicial -0.15
x(1,j)=px; % Se formaran matrices x de 1 fila x (1 hasta N1) columnas con el valor inicial 0.452
end
phi=zeros(1,N1); % La variable phi estara comprendida por 1 fila de N1 Ceros
for k=1:length(z) % La variable k ira desde 1 hasta el maximo valor de la variable z en valores de una unidad
 phik=phi(k);
T(:,:,k)=[cos(phik) -sin(phik) 0 x(k); % La matriz T es la matriz homogenea
sin(phik) cos(phik) 0 y(k);
0 0 1 z(k);
0 0 0 1];
end
qzz=ikine(robot,T) % ikine es una funcion de Cinematica Inversa del Toolbox de Robotica
plot(robot,qzz)

% Lineas de programacion para el segundo tramo de movimiento de N2 iteraciones en el eje Y

y=linspace(d3,d4,N2); % La variable y se distribuira desde -0.15 hasta 0.15 en N2 partes iguales (d4-d3=0.30 unidades)
x=zeros(1,N2); % La variable x estara comprendida por 1 fila de N2 Ceros
z=x; % La variable z sera igual a lo obtenido en la variable x (1 fila de N2 Ceros)
for j=1:N2 % La variable j ira desde 1 hasta N2 en valores de una unidad
x(1,j)=px; % Se formaran matrices x de 1 fila x (1 hasta N2) columnas con los valores 0.452 (X no ha sufrido cambios)
z(1,j)=pz+(d2-d1);% Se formara matrices z de 1 fila x (1 hasta N2) columnas con valores 0.512 (Punto final movimiento anterior)
end
phi=zeros(1,N2);
for k=1:length(y) % La variable k ira desde 1 hasta el maximo valor de la variable y en valores de una unidad
phik=phi(k);
T(:,:,k)=[cos(phik) -sin(phik) 0 x(k); % La matriz T es la matriz homogenea
sin(phik) cos(phik) 0 y(k);
0 0 1 z(k);
0 0 0 1];
end
qyy=ikine(robot,T) % ikine es una funcion de Cinematica Inversa del Toolbox de Robotica
plot(robot,qyy)

% Lineas de programacion para el tercer tramo de movimiento de N3 iteraciones en el eje X

x=linspace(d5,d6,N3); % La variable x se distribuira desde 0.452 hasta 0.502 en N3 partes iguales (d6-d5=0.05 unidades)
y=zeros(1,N3); % La variable y estara comprendida por 1 fila de N3 Ceros
z=y; % La variable z sera igual a lo obtenido en la variable y (1 fila de N3 Ceros)
for j=1:N3 % La variable j ira desde 1 hasta N3 en valores de una unidad
y(1,j)=py+(d4-d3);% Se formara matrices y de 1 fila x (1 hasta N3) columnas con los valores 0.05 (Y=py+(d4-d3)=-0.15+0.20=0.05)
z(1,j)=pz+(d2-d1);% Se formaran matrices z de 1 fila x (1 hasta N3) columnas con los valores 0.482 (Z no ha sufrido cambios)
end
phi=zeros(1,N3); % La variable phi estara comprendida por 1 fila de N3 Ceros
for k=1:length(x) % La variable k ira desde 1 hasta el maximo valor de la variable x en valores de una unidad
phik=phi(k);
T(:,:,k)=[cos(phik) -sin(phik) 0 x(k); % La matriz T es la matriz homogenea
sin(phik) cos(phik) 0 y(k);
0 0 1 z(k);
0 0 0 1];
end
qxx=ikine(robot,T) % ikine es una funcion de Cinematica Inversa del Toolbox de Robotica
plot(robot,qxx)

% Lineas de programacion para el cuarto tramo de movimiento de N4 iteraciones en el eje Y

y=linspace(d7,d8,N4); % La variable y se distribuira desde 0.05 hasta 0.30 en N4 partes iguales (d8-d7=0.15 unidades)
x=zeros(1,N4); % La variable x estara comprendida por 1 fila de N4 Ceros
z=x; % La variable z sera igual a lo obtenido en la variable x (1 fila de N Ceros)
for j=1:N4 % La variable j ira desde 1 hasta N4 en valores de una unidad
x(1,j)=px+(d6-d5);% Se formara matrices x de 1 fila x (1 hasta N4) columnas con valores 0.502 (X=px+(d6-d5)=0.452+0.25=0.502)
z(1,j)=pz+(d2-d1);% Se formaran matrices z de 1 fila x (1 hasta N4) columnas con los valores 0.512 (Z no ha sufrido cambios)
end
phi=zeros(1,N4); % La variable phi estara comprendida por 1 fila de N4 Ceros
for k=1:length(y) % La variable k ira desde 1 hasta el maximo valor de la variable y en valores de una unidad
phik=phi(k);
T(:,:,k)=[cos(phik) -sin(phik) 0 x(k); % La matriz T es la matriz homogenea
sin(phik) cos(phik) 0 y(k);
0 0 1 z(k);
0 0 0 1];
end
qyy=ikine(robot,T) % ikine es una funcion de Cinematica Inversa del Toolbox de Robotica
plot(robot,qyy)

Ventana del Command Window en MATLAB en donde se debe seguir los pasos que se indican de acuerdo a las líneas de programacion
7. Líneas del algoritmo del archivo: PUMA560_4B

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Archivo PUMA560_4B que se ejecuta necesariamente con la funcion ikine del Toolbox Robotic de MATLAB
% De acuerdo a la programacion el sistema se movera de la siguiente forma:
% Moverse 150 unidades en el eje X
% Moverse 30 unidades en el eje Y
% Moverse 5 unidades en el eje Z
% Moverse 150 unidades en el eje X
% Moverse 15 unidades en el eje Y
% Los valores iniciales en el eje XYZ del Robot Puma son X=0.452; Y=-0.150; Z=0.432
% Los valores de variacion de cada articulacion esta de la siguiente forma:
%
% Inicio Primer Movimiento Segundo Movimiento Tercer Movimiento Cuarto Movimiento Quinto Movimiento
% X=0.452 -----> 0.602 ------------------------------------------------------------> 0.452
% Y=-0.15 ---------------------------> 0.15 ------------------------------------------------------------> 0.30
% Z=0.432 --------------------------------------------------> 0.482
%
% La variable N sera el numero de iteraciones (Velocidad de movimiento) y cada linea de programacion con el comando:
% linspace(d1,d2,N) sera las unidades de cantidad de movimiento a ser realizado
%
% Escribir en el Command Window las siguientes lineas:
% puma560
% drivebot(p560)
%
% INGENIERO ELECTRONICO MONTEZA ZEVALLOS FIDEL TOMAS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear; % Limpiamos el Workspace

clc; % Limpiamos el Command Window
close all; % Cerramos todo

puma560; % Se llama al proyecto Robot Puma creado en MATLAB

robot=p560

fprintf('\n CINEMATICA INVERSA DEL ROBOT PUMA 560 EN MATLAB') % Con (\n) se imprime en la siguiente linea
fprintf('\n ') % Con (\n) se imprime en la siguiente linea
fprintf('\n INGRESE LOS DATOS ESTABLECIDOS PARA GENERAR EL MOVIMIENTO') % Con (\n) se imprime en la siguiente linea
fprintf('\n ') % Con (\n) se imprime en la siguiente linea

fprintf('\n El valor de N1 (Numero de iteraciones del Primer Movimiento podria ser 40)') % (\n) se imprime en la siguiente linea
N1=input('\n INGRESE N1 = '); % La variable N1 es el numero de iteraciones para el eje X
fprintf('\n El valor de N2 (Numero de iteraciones del Segundo Movimiento podria ser 70)') % (\n) se imprime en la siguiente linea
N2=input('\n INGRESE N2 = '); % La variable N2 es el numero de iteraciones para el eje Y
fprintf('\n El valor de N3 (Numero de iteraciones del Tercer Movimiento podria ser 90)') % (\n) se imprime en la siguiente linea
N3=input('\n INGRESE N3 = '); % La variable N3 es el numero de iteraciones para el eje Z
fprintf('\n El valor de N4 (Numero de iteraciones del Cuarto Movimiento podria ser 120)') % (\n) se imprime en la siguiente linea
N4=input('\n INGRESE N4 = '); % La variable N4 es el numero de iteraciones para el eje X
fprintf('\n El valor de N5 (Numero de iteraciones del Quinto Movimiento podria ser 130)') % (\n) se imprime en la siguiente linea
N5=input('\n INGRESE N5 = '); % La variable N5 es el numero de iteraciones para el eje Y
fprintf('\n ') % Con (\n) se imprime en la siguiente linea

fprintf('\n USTED INGRESARA LOS DATOS DE LA POSICION INICIAL DE ROBOT PUMA 560') % Con (\n) se imprime en la siguiente linea
fprintf('\n ') % Con (\n) se imprime en la siguiente linea
fprintf('\n El valor de px (Valor inicial en el eje X del Robot Puma es 0.452)') % Con (\n) se imprime en la siguiente linea
px=input('\n INGRESE px = '); % La variable px es el Valor inicial en el eje X del Robot Puma
fprintf('\n El valor de py (Valor inicial en el eje Y del Robot Puma es -0.150)') % Con (\n) se imprime en la siguiente linea
py=input('\n INGRESE py = '); % La variable py es el Valor inicial en el eje Y del Robot Puma
fprintf('\n El valor de pz (Valor inicial en el eje Z del Robot Puma es 0.432)') % Con (\n) se imprime en la siguiente linea
pz=input('\n INGRESE pz = '); % La variable pz es el Valor inicial en el eje Z del Robot Puma
fprintf('\n ') % Con (\n) se imprime en la siguiente linea

fprintf('\n USTED INGRESARA DATOS DE LAS POSICIONES INICIAL Y FINAL DE ROBOT PUMA 560') % Con (\n) se imprime en la siguiente linea
fprintf('\n ') % (\n) se imprime en la siguiente linea
fprintf('\n El valor de d1 (Punto inicial del primer movimiento del brazo robotico, puede ser 0.452)')
d1=input('\n INGRESE d1 = '); % Punto inicial del primer movimiento
fprintf('\n El valor de d2 (Punto final del segundo movimiento del brazo robotico, puede ser 0.602)')
d2=input('\n INGRESE d2 = '); % Punto final del primer movimiento
fprintf('\n El valor de d3 (Punto inicial del segundo movimiento del brazo robotico, puede ser -0.15)')
d3=input('\n INGRESE d3 = '); % Punto inicial del segundo movimiento
fprintf('\n El valor de d4 (Punto final del segundo movimiento del brazo robotico, puede ser 0.15)')
d4=input('\n INGRESE d4 = '); % Punto final del segundo movimiento
fprintf('\n El valor de d5 (Punto inicial del tercer movimiento del brazo robotico, puede ser 0.432)')
d5=input('\n INGRESE d5 = '); % Punto inicial del tercer movimiento
fprintf('\n El valor de d6 (Punto final del tercer movimiento del brazo robotico, puede ser 0.482)')
d6=input('\n INGRESE d6 = '); % Punto final del tercer movimiento
fprintf('\n El valor de d7 (Punto inicial del cuarto movimiento del brazo robotico, puede ser 0.602)')
d7=input('\n INGRESE d7 = '); % Punto inicial del cuarto movimiento
fprintf('\n El valor de d8 (Punto final del cuarto movimiento del brazo robotico, puede ser 0.452)')
d8=input('\n INGRESE d8 = '); % Punto final del cuarto movimiento
fprintf('\n El valor de d9 (Punto inicial del primer movimiento del brazo robotico, puede ser 0.15)')
d9=input('\n INGRESE d9 = '); % Punto inicial del primer movimiento
fprintf('\n El valor de d10 (Punto final del segundo movimiento del brazo robotico, puede ser 0.30)')
d10=input('\n INGRESE d10 = '); % Punto final del primer movimiento
% Lineas de programacion para el primer tramo de movimiento de N1 iteraciones en el eje X
x=linspace(d1,d2,N1); % La variable x se distribuira desde 0.602 hasta 0.452 en N1 partes iguales (d2-d1=0.150 unidades)
y=zeros(1,N1); % La variable y estara comprendida por 1 fila de N1 Ceros
z=x; % La variable z sera igual a lo obtenido en la variable x (1 fila de N1 Ceros)
for j=1:N1 % La variable j ira desde 1 hasta N1 en valores de una unidad
y(1,j)=py; % Se formaran matrices y de 1 fila x (1 hasta N1) columnas con el valor inicial -0.15
z(1,j)=pz; % Se formaran matrices z de 1 fila x (1 hasta N1) columnas con el valor inicial 0.432
end
phi=zeros(1,N1); % La variable phi estara comprendida por 1 fila de N1 Ceros
for k=1:length(x) % La variable k ira desde 1 hasta el maximo valor de la variable x en valores de una unidad
phik=phi(k);
T(:,:,k)=[cos(phik) -sin(phik) 0 x(k); % La matriz T es la matriz homogenea
sin(phik) cos(phik) 0 y(k);
0 0 1 z(k);
0 0 0 1];
end
qxx=ikine(robot,T) % ikine es una funcion de Cinematica Inversa del Toolbox de Robotica
plot(robot,qxx)

% Lineas de programacion para el segundo tramo de movimiento de N2 iteraciones en el eje Y

y=linspace(d3,d4,N2); % La variable y se distribuira desde -0.15 hasta 0.55 en N2 partes iguales (d4-d3=0.30 unidades)
x=zeros(1,N2); % La variable x estara comprendida por 1 fila de N2 Ceros
z=x; % La variable z sera igual a lo obtenido en la variable x (1 fila de N2 Ceros)
for j=1:N2 % La variable j ira desde 1 hasta N2 en valores de una unidad
x(1,j)=px+(d2-d1); % Se formaran matrices x de 1 fila x (1 hasta N2) columnas con los valores 0.602 (X no ha sufrido cambios)
z(1,j)=pz; % Se formaran matrices z de 1 fila x (1 hasta N2) columnas con los valores 0.432 (Punto final movimiento anterior)
end
phi=zeros(1,N2);
for k=1:length(y) % La variable k ira desde 1 hasta el maximo valor de la variable y en valores de una unidad
phik=phi(k);
T(:,:,k)=[cos(phik) -sin(phik) 0 x(k); % La matriz T es la matriz homogenea
sin(phik) cos(phik) 0 y(k);
0 0 1 z(k);
0 0 0 1];
end
qyy=ikine(robot,T) % ikine es una funcion de Cinematica Inversa del Toolbox de Robotica
plot(robot,qyy)

% Lineas de programacion para el tercer tramo de movimiento de N3 iteraciones en el eje Z

z=linspace(d5,d6,N3); % La variable z se distribuira desde 0.432 hasta 0.482 en N3 partes iguales (d6-d5=0.05 unidades)
y=zeros(1,N3); % La variable y estara comprendida por 1 fila de N3 Ceros
x=y; % La variable x sera igual a lo obtenido en la variable y (1 fila de N3 Ceros)
for j=1:N3 % La variable j ira desde 1 hasta N3 en valores de una unidad
y(1,j)=py+(d4-d3);% Se formara matrices y de 1 fila x (1 hasta N3) columnas con valores 0.05 (Y=py+(d4-d3)=-0.15+0.20=0.05)
x(1,j)=px+(d2-d1);% Se formaran matrices z de 1 fila x (1 hasta N3) columnas con los valores 0.482 (Z no ha sufrido cambios)
end
phi=zeros(1,N3); % La variable phi estara comprendida por 1 fila de N3 Ceros
for k=1:length(z) % La variable k ira desde 1 hasta el maximo valor de la variable x en valores de una unidad
phik=phi(k);
T(:,:,k)=[cos(phik) -sin(phik) 0 x(k); % La matriz T es la matriz homogenea
sin(phik) cos(phik) 0 y(k);
0 0 1 z(k);
0 0 0 1];
end
qzz=ikine(robot,T) % ikine es una funcion de Cinematica Inversa del Toolbox de Robotica
plot(robot,qzz)

% Lineas de programacion para el cuarto tramo de movimiento de N4 iteraciones en el eje X

x=linspace(d7,d8,N4); % La variable x se distribuira desde 0.05 hasta -0.05 en N4 partes iguales (d8-d7=0.10 unidades)
y=zeros(1,N4); % La variable y estara comprendida por 1 fila de N4 Ceros
z=x; % La variable z sera igual a lo obtenido en la variable x (1 fila de N4 Ceros)
for j=1:N4 % La variable j ira desde 1 hasta N4 en valores de una unidad
y(1,j)=py+(d4-d3);% Se formara matrices x de 1 fila x (1 hasta N4) columnas con valores 0.702 (X=px+(d6-d5)=0.452+0.25=0.702)
z(1,j)=pz+(d6-d5);% Se formaran matrices z de 1 fila x (1 hasta N4) columnas con los valores 0.482 (Z no ha sufrido cambios)
end
phi=zeros(1,N4); % La variable phi estara comprendida por 1 fila de N4 Ceros
for k=1:length(x) % La variable k ira desde 1 hasta el maximo valor de la variable y en valores de una unidad
phik=phi(k);
T(:,:,k)=[cos(phik) -sin(phik) 0 x(k); % La matriz T es la matriz homogenea
sin(phik) cos(phik) 0 y(k);
0 0 1 z(k);
0 0 0 1];
end
qxx=ikine(robot,T) % ikine es una funcion de Cinematica Inversa del Toolbox de Robotica
plot(robot,qxx)

% Lineas de programacion para el quinto tramo de movimiento de N5 iteraciones en el eje Y

y=linspace(d9,d10,N5); % La variable x se distribuira desde 0.05 hasta -0.05 en N5 partes iguales (d8-d7=0.10 unidades)
x=zeros(1,N5); % La variable y estara comprendida por 1 fila de N5 Ceros
z=x; % La variable z sera igual a lo obtenido en la variable x (1 fila de N5 Ceros)
for j=1:N5 % La variable j ira desde 1 hasta N5 en valores de una unidad
x(1,j)=px+(d2-d1)+(d8-d7); % Se formara matrices x de 1 fila x (1 hasta N4) columnas con valores 0.702 (X=px+(d6-
d5)=0.452+0.25=0.702)
z(1,j)=pz+(d6-d5);% Se formaran matrices z de 1 fila x (1 hasta N4) columnas con los valores 0.482 (Z no ha sufrido cambios)
end
phi=zeros(1,N4); % La variable phi estara comprendida por 1 fila de N5 Ceros
for k=1:length(y) % La variable k ira desde 1 hasta el maximo valor de la variable y en valores de una unidad
phik=phi(k);
T(:,:,k)=[cos(phik) -sin(phik) 0 x(k); % La matriz T es la matriz homogenea
 sin(phik) cos(phik) 0 y(k);
0 0 1 z(k);
0 0 0 1];
end
qyy=ikine(robot,T) % ikine es una funcion de Cinematica Inversa del Toolbox de Robotica
plot(robot,qyy)