reporte de tarea 1 del mcdp para motoman es280d

INSTITUTO TECNOLÓGICO DE LA LAGUNA

Departamento de Estudios de Posgrado e Investigación

Maestría en Ingeniería Eléctrica

Línea de investigación: Mecatrónica y control

Semestre: Primero

Nombre de la materia: Robótica Nombre del profesor: Dr. José Alfonso Pámanes García

Tarea 1:

Simulación en Matlab del Modelo Cinemático Directo del robot Motoman ES280D

Nombre del alumno: Pedro Zúñiga Flores Número de control: M07131483 Fecha de entrega: 30/mar/2012

ENERO – JUNIO 2012 TORREÓN, COAH.

OBJETIVO

Obtener los siguientes puntos:

1- Esquema cinemático

2- PDH modificados

3- Matrices elementales

4- Ecuaciones del MCD de posición (X06, Y06, Z06, α, β, γ)

5- Modelo del robot para animación

6- Efectuar la simulación para las siguientes trayectorias articulares:

𝜃𝑖 = 𝜃𝑖 𝑖𝑛𝑖+ 𝛿𝜃𝑖 ∙ 𝑓𝜃 𝑡 𝑝𝑎𝑟𝑎 𝑖 = 1, 2, ⋯ , 𝑛

INTRODUCCIÓN

El modelo cinemático directo de posición (MCDP) consiste en el establecimiento de las

funciones que hacen posible determinar las coordenadas operacionales de un robot a partir

de sus coordenadas articulares. En el caso que se presentará en este estudio, se analizará el

MCDP de un robot manipulador de 6 grados de libertad (6 gdl) con el cual se desarrollará

una animación en Matlab para ejecutar varias trayectorias articulares del robot dentro de

sus límites articulares dados por el fabricante. Este robot del cual se habla, se trata del

Motoman ES280D, que es un manipulador serial de 6 gdl que está diseñado para el manejo

de materiales no mayores de 280 kg con un rango máximo de trabajo de 2.446 m. A

continuación en la Figura 1 se presenta un diagrama del espacio de trabajo de este robot

donde se representa el alcance de operación del Motoman ES280D además de sus

dimensiones físicas y algunos de sus límites articulares.

Figura 1. Espacio de trabajo de Motoman ES280D y configuración inicial.

En la siguiente Tabla 1 se especifican los límites articulares representados por letras

mayúsculas en la Figura 1.

Ejes Rango máximo de movimiento (grados)

S ± 180

L +76/-60

U +230/-142.5

R ± 360

B ±125

T ± 360

Tabla 1. Límites articulares

1. Esquema cinemático

El esquema cinemático del robot Motoman ES280D se obtendrá a partir del siguiente

diagrama de alambre de la Figura 2.

Figura 2. Diagrama de alambre del robot Motoman ES280D.

Las distancias del diagrama de alambre son las que se muestran en la Tabla 2, las cuales

son extraídas de la Figura 1.

Longitudes Unidades (cm)

L1 65

L2 28.5

L3 115

L4 25

L5 101.5

L6 25

Tabla 2. Longitudes del robot.

Para obtener el esquema cinemático a partir del diagrama de alambre, se seguirá el método

convencional de asignación de los ejes (x, y, z) para cada eslabón de la cadena cinemática

del robot. Este método convencional se describe a continuación:

El eje zj se define a lo largo del eje de la articulación aj. Este eje es el de rotación en

el caso de una articulación tipo R (rotatoria), o el de deslizamiento de la corredera

en el caso de una articulación tipo P. En este último caso la posición de zj se fija

arbitrariamente.

El eje xj se define a lo largo de la perpendicular común a zj y a zj-1. Si estos dos ejes

son paralelos, xj no se puede definir de manera única; en tal caso se recomienda

seguir un criterio de simetría o de simplicidad para definir la ubicación de este eje.

Por supuesto, el punto de intersección de xj y zj definen al origen oj del marco.

El eje yj se define a partir de los ejes xj y zj, de tal manera que se complete un marco

de mano derecha.

Nótese que mediante esta convención no es posible definir los marcos unidos a los

eslabones C0 y Cn (eslabón fijo de la base y eslabón correspondiente a la ultima

articulación). Tales marcos se pueden especificar de manera simple como sigue: el marco

∑0 se escoge de tal manera que este alineado con el ∑1 cuando la variable articular q1 sea

nula; por su parte, el marco ∑n se elige de modo que este alineado con el ∑n-1 cuando qn=0.

A continuación se presentan los marcos asignados al diagrama de alambre en la Figura 3.

Figura 3. Esquema cinemático del manipulador Motoman ES280D.

Los marcos con subíndices 0, 1, 2, 3, 4, 5 y 6 son los que se obtuvieron siguiendo el

método convencional para la ubicación de los ejes (x, y, z) en cada eslabón de la Figura 3.

Los marcos con subíndices ‘a’ y ’h’ se colocaron arbitrariamente en una posición (x, y, z)

de la cadena cinemática del robot y están referenciados al marco predecesor, ya que tienen

la misma orientación que el marco que los antecede solo que una distancia de separación en

alguno de sus ejes con respecto al marco anterior. A estos marcos Oa y Oh les llamaremos

marcos auxiliares ya que se utilizarán más adelante en el modelo de animación del robot en

Matlab.

Una vez que se obtuvo el esquema cinemático del manipulador Motoman ES280D, se

procederá a obtener los parámetros de Denavit-Hartenberg Modificados (PDHM) tomando

como referencia la Figura 3.

2. Parámetros de Denavit-Hartenberg Modificados (PDHM).

Una vez asignado un marco a cada eslabón, se definirán los parámetros geométricos que

especifican la posición y orientación de cada marco respecto al precedente. Para esto se

seguirán las siguientes especificaciones de dichos parámetros:

αj Es el ángulo de zj-1 a zj, medido respecto a xj-1 conforme a la regla de la

mano derecha.

dj Es la distancia entre zj-1 y zj, a lo largo de xj-1.

θj Es el ángulo de xj-1 a xj, medido respecto a zj conforme a la regla de la mano

derecha.

rj Es la distancia entre xj-1 y xj a lo largo de zj.

σj Esta variable vale cero si la articulación j es tipo R (rotacional) y vale uno si

la articulación j es de tipo P (prismática).

Los PDHM obtenidos de la Figura 3 son los que se muestran en la Tabla 3.

j σj αj (grados) dj θj rj

1 0 0 0 θ1 0

2 0 90 d2 θ2 0

3 0 0 d3 θ3 0

4 0 90 d4 θ4 r4

5 0 -90 0 θ5 0

6 0 90 0 θ6 0

Tabla 3. Parámetros de Denavit Hartenberg Modificados (PDHM).

Donde las distancias d2, d3 y d4 corresponden a las longitudes L2, L3 y L4 respectivamente,

y la distancia r4 corresponde a la longitud L5, según las Figuras 2 y 3.

Una vez que se obtuvieron los PDHM del esquema cinemático del manipulador Motoman

ES280D, se procederá a obtener las matrices elementales del robot.

3. Matrices elementales.

Para obtener las matrices elementales del robot Motoman ES280D, haremos uso de la

matriz de transformación homogénea general que defina, en función de los PDHM, la

posición y orientación del marco ∑j respecto al ∑j-1. Esta matriz es la siguiente:

𝑇𝑗𝑗−1

=

𝑐𝜃𝑗𝑐𝛼𝑗 𝑠𝜃𝑗𝑠𝛼𝑗 𝑠𝜃𝑗

−𝑠𝜃𝑗𝑐𝛼𝑗 𝑐𝜃𝑗𝑠𝛼𝑗 𝑐𝜃𝑗

0 0

0−𝑠𝛼𝑗

𝑐𝛼𝑗

𝑑𝑗

−𝑟𝑗 𝑠𝛼𝑗

𝑟𝑗 𝑐𝛼𝑗

0 1

Nota: Por simplicidad se denota a la función seno con la letra ‘s’ y a la función coseno con

la letra ‘c’ en la matriz anterior.

Sustituyendo en la matriz anterior los PDHM de la Tabla 3, pertenecientes a cada eslabón

del robot, se obtienen las siguientes seis matrices de transformación homogéneas, las cuales

son llamadas matrices elementales del robot.

𝑇10 =

𝑐1

𝑠1

0

−𝑠1

𝑐1

00 0

001

000

0 1

𝑇21 =

𝑐2

0𝑠2

−𝑠2

0𝑐2

0 0

0−10

𝑑2

00

0 1

𝑇32 =

𝑐3

𝑠3

0

−𝑠3

𝑐3

00 0

001

𝑑3

00

0 1

𝑇43 =

𝑐4

0𝑠4

−𝑠4

0𝑐4

0 0

0−10

𝑑4

−𝑟4

00 1

𝑇54 =

𝑐5

0−𝑠5

−𝑠5

0−𝑐5

0 0

010

000

0 1

𝑇65 =

𝑐6

0𝑠6

−𝑠6

0𝑐6

0 0

0−10

000

0 1

Nota: Las matrices elementales están escritas de manera simplificada, tomando en cuenta la

siguiente notación de la Tabla 4.

s1=sin(θ1)

c1=cos(θ1)

s2=sin(θ2)

c2=cos(θ2)

s3=sin(θ3)

c3=cos(θ3)

s4=sin(θ4)

c4=cos(θ4)

s5=sin(θ5)

c5=cos(θ5)

s6=sin(θ6)

c6=cos(θ6)

Tabla 4. Notación de las funciones seno y coseno.

Una vez obtenidas las matrices elementales del robot Motoman ES280D, se procederá a

obtener una matriz de transformación homogénea que defina la posición y orientación del

marco ∑6 con respecto al marco de la base del robot ∑0. Esto para obtener las ecuaciones

del Modelo Cinemático Directo de posición.

4. Ecuaciones del MCD de posición (X06, Y06, Z06, α, β, γ)

La matriz que define las ecuaciones del MCD de posición es la resultante de multiplicar

matricialmente las matrices elementales del robot como se muestra a continuación:

𝑇 = 𝑇10 ∙ 𝑇2

1 ∙ 𝑇32 ∙ 𝑇4

3 ∙ 𝑇54 ∙ 𝑇6

560

Por la complejidad de las operaciones se decide hacer las multiplicaciones matriciales en

MATLAB, y el resultado se dividió en las 12 ecuaciones que definen la posición y

orientación del marco ∑6 con respecto al marco ∑0, y que forman parte de los elementos de

renglones y columnas de la matriz 𝑇60 .

T11=s6*(c4*s1 + s4*(c1*s2*s3 - c1*c2*c3)) + c6*(c5*(s1*s4 - c4*(c1*s2*s3 - c1*c2*c3)) + s5*(c1*c2*s3 + c1*c3*s2))

T12=c6*(c4*s1 + s4*(c1*s2*s3 - c1*c2*c3)) - s6*(c5*(s1*s4 - c4*(c1*s2*s3 - c1*c2*c3)) + s5*(c1*c2*s3 + c1*c3*s2))

T13=c5*(c1*c2*s3 + c1*c3*s2) - s5*(s1*s4 - c4*(c1*s2*s3 - c1*c2*c3))

T14= c1*(d2 + d4*c23 + r4*s23 + d3*c2)

T21=- s6*(c1*c4 - s4*(s1*s2*s3 - c2*c3*s1)) - c6*(c5*(c1*s4 + c4*(s1*s2*s3 - c2*c3*s1)) - s5*(c2*s1*s3 + c3*s1*s2))

T22=s6*(c5*(c1*s4 + c4*(s1*s2*s3 - c2*c3*s1)) - s5*(c2*s1*s3 + c3*s1*s2)) - c6*(c1*c4 - s4*(s1*s2*s3 - c2*c3*s1))

T23=s5*(c1*s4 + c4*(s1*s2*s3 - c2*c3*s1)) + c5*(c2*s1*s3 + c3*s1*s2)

T24=s1*(d2 + d4*c23 + r4*s23 + d3*c2)

T31=- c6*(s5*(c2*c3 - s2*s3) - c4*c5*(c2*s3 + c3*s2)) - s4*s6*(c2*s3 + c3*s2)

T32=s6*(s5*(c2*c3 - s2*s3) - c4*c5*(c2*s3 + c3*s2)) - c6*s4*(c2*s3 + c3*s2)

T33= - c5*(c2*c3 - s2*s3) - c4*s5*(c2*s3 + c3*s2)

T34= d4*s23 - r4*c23 + d3*s2

En estas 12 ecuaciones anteriores de T11 a T34, se sigue respetando la notación de la Tabla

4, tomando en cuenta además la siguiente notación:

s23=sin(θ2+ θ3)

c23=cos(θ2+ θ3)

Las ecuaciones que definen al MCD de posición son las expresadas en las ecuaciones T14,

T24 y T34 para el vector de posición, mas las ecuaciones que ayudan a definir a los ángulos

α, β y γ (alpha, beta y gamma) para obtener la matriz de orientación siguiendo la

convención de ángulos de Euler.

Los ángulos de Euler corresponden a 3 rotaciones sucesivas de los tres ejes en el orden z, y,

x, llamadas alpha, beta y gamma, que necesitan aplicarse al marco ∑0 (marco fijo de la

base) para hacerlo coincidir con el marco del ultimo eslabón ∑6.

Estos 3 ángulos de Euler los obtendremos igualando la matriz de orientación del MCDP

con la matriz de ángulos de Euler que a continuación se presenta:

𝑐𝛼𝑐𝛽 𝑐𝛼𝑠𝛽𝑠𝛾 − 𝑠𝛼𝑐𝛾 𝑐𝛼𝑠𝛽𝑐𝛾 − 𝑠𝛼𝑠𝛾𝑠𝛼𝑐𝛽 𝑠𝛼𝑠𝛽𝑠𝛾 + 𝑐𝛼𝑐𝛾 𝑠𝛼𝑠𝛽𝑐𝛾 − 𝑐𝛼𝑠𝛾−𝑠𝛽 𝑐𝛽𝑠𝛾 𝑐𝛽𝑐𝛾

Y al igualar la matriz de ángulos de Euler con la matriz de rotación del MCDP, se obtiene:

𝑇11 𝑇12 𝑇13𝑇21 𝑇22 𝑇23𝑇31 𝑇32 𝑇33

=

𝑐𝛼𝑐𝛽 𝑐𝛼𝑠𝛽𝑠𝛾 − 𝑠𝛼𝑐𝛾 𝑐𝛼𝑠𝛽𝑐𝛾 − 𝑠𝛼𝑠𝛾𝑠𝛼𝑐𝛽 𝑠𝛼𝑠𝛽𝑠𝛾 + 𝑐𝛼𝑐𝛾 𝑠𝛼𝑠𝛽𝑐𝛾 − 𝑐𝛼𝑠𝛾−𝑠𝛽 𝑐𝛽𝑠𝛾 𝑐𝛽𝑐𝛾

A partir de la igualación anterior se pueden despejar los ángulos de Euler de las ecuaciones

de ambas matrices como se muestra a continuación:

𝛼 = 𝑎𝑡𝑎𝑛2 𝑇21 ,𝑇11

𝛽 = 𝑎𝑡𝑎𝑛2 −𝑇31 ,𝑇11

𝑐𝛼

𝛾 = 𝑎𝑡𝑎𝑛2(𝑇32 ,𝑇33)

Estos tres ángulos de Euler anteriores, son los que me definen las rotaciones sucesivas (en

el orden z, y, x) del marco∑6 con respecto al marco ∑0.

Y como ya se dijo anteriormente las ecuaciones T14, T24 y T34 son las que me definen el

vector de posición del último marco ∑6 con respecto al marco ∑0.

𝑥06 𝑦06 𝑧06 ′ = 𝑇14 𝑇24 𝑇34 ′

Las últimas 4 ecuaciones arriba descritas conforman el MCDP del robot Motoman

ES280D. El siguiente punto a tratar es sobre el modelo del robot para animación en Matlab.

5. Modelo del robot para animación.

Para la animación del robot Motoman ES280D en Matlab se considerarán los marcos

auxiliares ∑a y ∑h que se colocaron arbitrariamente en la Figura 3 del esquema cinemático

del robot. Las matrices de transformación homogéneas para estos marcos con respecto a su

marco predecesor son las siguientes:

𝑇 =𝑎3

100

010

0 0

001

𝑑4

00

0 1

𝑇 =ℎ6

100

010

0 0

001

00𝑟6

0 1

Nótese que las 2 matrices de transformación homogéneas anteriores poseen una submatriz

identidad correspondiente a la matriz de rotación de cada una, debido a que los marcos

auxiliares ∑a y ∑h tienen la misma orientación que los marcos ∑3 y ∑6 respectivamente,

solo que los marcos auxiliares están desplazados una distancia en un eje con respecto a su

marco predecesor como se ve en sus matrices de transformación. Las distancias d4 y r6

corresponden a las longitudes L4 y L6 respectivamente de la Figura 2.

Para la graficación de la ruta del centro de la herramienta de la Figura 3, se considerará un

marco ∑ch en el centro de la herramienta, el cual posee la siguiente matriz de

transformación con respecto a su marco predecesor (∑h).

𝑇 =𝑐ℎℎ

100

010

0 0

001

004

0 1

En la siguiente página se presenta el código de simulación escrito en un archivo Script de

Matlab, en el cual se describe detalladamente las instrucciones para realizar el modelo de

animación del robot.

% Programa que efectúa la animación del robot Motoman ES280D % y muestra la ruta que describe el órgano terminal % Programa realizado por J. Alfonso Pámanes García % Última actualización: Mar 27/2012

% Parámetros geométricos del robot: d2=28.5;%Igual a L2 d3=115;%Igual a L3 d4=25;%Igual a L4 r4=101.5;%Igual a L5 r6=25;%Igual a L6 rch=4;%distancia del inicio de la herramienta al centro de la herramienta

% Emplazamiento del robot: POSICION Y ORIENTACION DE LOS MARCOS CON % RESPECTO LA ESTACION DE TRABAJO (EL MARCO DEL MUNDO DEL MATLAB) aa=0; bb=0; cc=0; %Matriz de emplazamiento Te0=[0 -1 0 aa 1 0 0 bb 0 0 1 cc 0 0 0 1 ];

% Parámetros de graficación: np=50;

% Parámetros del movimiento del robot:

T=5;

% Configuracion inicial del robot segun el fabricante: th1i=0;%ANGULOS INICIALES EN CADA ARTICULACION th2i=pi/2; th3i=0; th4i=0; th5i=0; th6i=0;

delth1g=180;%ANGULOS EN GRADOS A INCREMENTAR EN CADA ARTICULACION delth2g=0; delth3g=0; delth4g=0; delth5g=0; delth6g=0;

delth1=delth1g*pi/180;%Conversion de grados a radianes delth2=delth2g*pi/180; delth3=delth3g*pi/180; delth4=delth4g*pi/180; delth5=delth5g*pi/180; delth6=delth6g*pi/180;

for i=0:np t=T*i/np;%Ecuacion de funcion del tiempo funt = (t/T)-(1/(2*pi))*(sin(2*pi*t/T)); th1 = th1i+delth1*funt; th2 = th2i+delth2*funt; th3 = th3i+delth3*funt; th4 = th4i+delth4*funt; th5 = th5i+delth5*funt; th6 = th6i+delth6*funt; th12 = th1+th2; th123 = th12+th3; th1234= th123+th4; th12345= th1234+th5; th123456= th12345+th6;

%%%%%%%%%%%%%%%%%%%%%%%%%%% %Notacion matematica a utilizar s1=sin(th1); c1=cos(th1); s2=sin(th2); c2=cos(th2); s3=sin(th3); c3=cos(th3); s4=sin(th4); c4=cos(th4); s5=sin(th5); c5=cos(th5); s6=sin(th6); c6=cos(th6);

s12=sin(th12); c12=cos(th12); c23=cos(th2+th3); s23=sin(th2+th3); s45=sin(th4+th5); s54=sin(th4-th5); s123=sin(th123); c123=cos(th123); s1234=sin(th1234); c1234=cos(th1234);

%xch,ych,zch, son el vector de posicion del centro de la herramienta %con respecto al marco cero de la base. Este vector se obtiene a

partir %de: T0ch=T01*T12*T23*T34*T45*T56*T6h*Thch xch=r6*(s5*(s1*s4 - c4*(c1*s2*s3 - c1*c2*c3)) + c5*(c1*c2*s3 +

c1*c3*s2)) + rch*(s5*(s1*s4 - c4*(c1*s2*s3 - c1*c2*c3)) + c5*(c1*c2*s3 +

c1*c3*s2)) + c1*(d2 + d4*c23 + r4*s23 + d3*c2);

ych=s1*(d2 + d4*c23 + r4*s23 + d3*c2) - rch*(s5*(c1*s4 + c4*(s1*s2*s3

- c2*c3*s1)) - c5*(c2*s1*s3 + c3*s1*s2)) - r6*(s5*(c1*s4 + c4*(s1*s2*s3 -

c2*c3*s1)) - c5*(c2*s1*s3 + c3*s1*s2));

zch=d4*s23 - r4*c23 + d3*s2 - rch*(c5*(c2*c3 - s2*s3) - c4*s5*(c2*s3

+ c3*s2)) + (r6*s23*s5)/2 - r6*c23*c5 - (r6*s54*s23)/2;

rpost=Te0*[xch ych zch 1]';%para obtener las cordenadas del vector

de posicion % del centro de la herramienta respecto al

marco de estacion de trabajo xpn=rpost(1); ypn=rpost(2); zpn=rpost(3);

pn1(i+1)=xpn;%SE UTILIZAN PARA DIBUJAR LA TRAZA DE LA RUTA DE LA

PINZA pn2(i+1)=ypn; pn3(i+1)=zpn;

% figure (1) %Para ubicar el vector de posicion de cada marco con respecto %a la base del robot x2=d2*c1;%Coordenadas de marco 2 en marco cero y2=d2*s1;%T02=T01*T12 z2=0;% x3=d2*c1+d3*c1*c2;%Coordenadas de marco 3 en marco cero y3=d2*s1+d3*s1*c2;%T03=T02*T23 z3=d3*s2;% xa=c1*(d2 + d4*c23 + d3*c2);%Coordenadas de marco a en marco cero ya=s1*(d2 + d4*c23 + d3*c2);%T0a=T03*T3a za=d4*s23 + d3*s2;% x4=c1*(d2 + d4*c23 + r4*s23 + d3*c2);%Coordenadas de marco 6 en marco

cero y4=s1*(d2 + d4*c23 + r4*s23 + d3*c2);%T04=T05=T06=T03*T34*T45*T56 z4=d4*s23 - r4*c23 + d3*s2;% xh=c1*(d2 + d4*c23 + r4*s23 + d3*c2) + r6*(s5*(s1*s4 - c4*(c1*s2*s3 -

c1*c2*c3)) + c5*(c1*c2*s3 + c1*c3*s2));%Coordenadas de marco h en marco

cero yh=-r6*(s5*(c1*s4 + c4*(s1*s2*s3 - c2*c3*s1)) - c5*(c2*s1*s3 +

c3*s1*s2)) + s1*(d2 + d4*c23 + r4*s23 + d3*c2);%T0h=T06*T6h zh=d4*s23 - r4*c23 + d3*s2 + (r6*s23*s45)/2 - r6*c23*c5 -

(r6*s54*s23)/2;%

rposp2=Te0*[x2 y2 z2 1]'; rposp3=Te0*[x3 y3 z3 1]'; rpospa=Te0*[xa ya za 1]'; rposp4=Te0*[x4 y4 z4 1]'; rposp4a=Te0*[x4 y4 z4 1]';%Para dibujar un circulo en marco 4, 5 y

6 rposph=Te0*[xh yh zh 1]';

%DIBUJA LINEAS PARA LOS ESLABONES r2x=[aa rposp2(1)];%Longitud L2 r2y=[bb rposp2(2)]; r2z=[cc rposp2(3)];% r3x=[rposp2(1) rposp3(1)];%Longitud L3 r3y=[rposp2(2) rposp3(2)]; r3z=[rposp2(3) rposp3(3)];% rax=[rposp3(1) rpospa(1)];%Longitud L4 ray=[rposp3(2) rpospa(2)]; raz=[rposp3(3) rpospa(3)];% r4x=[rpospa(1) rposp4(1)];%Longitud L5

r4y=[rpospa(2) rposp4(2)]; r4z=[rpospa(3) rposp4(3)];% r4ax=[rposp4(1) rposp4a(1)];%Para dibujar un circulo en marco 4, 5 y

6 r4ay=[rposp4(2) rposp4a(2)]; r4az=[rposp4(3) rposp4a(3)];% rhx=[rposp4a(1) rposph(1)];%Longitud L6 rhy=[rposp4a(2) rposph(2)]; rhz=[rposp4a(3) rposph(3)];%

% Base del robot:

sp1=[0 0 0 1]';%Longitud L1 sp2=[0 0 -65 1]';% b1=[-20 20 -65 1]';%4 puntos de la base del robot b2=[ 20 20 -65 1]'; b3=[ 20 -20 -65 1]'; b4=[-20 -20 -65 1]';

s1n=Te0*sp1;%Se multiplican por la matriz de emplazamiento para en caso

de s2n=Te0*sp2;%querer mover la base del robot de lugar con referencia a la b1n=Te0*b1;%estacion de trabajo que es el marco o mundo de Matlab b2n=Te0*b2; b3n=Te0*b3; b4n=Te0*b4;

%COMPONENTES X,Y,Z DEL PUNTO INICIAL Y PUNTO FINAL DE LA LINEA DE SOPORTE s1x=[s1n(1) s2n(1)];%Longitud L1 s1y=[s1n(2) s2n(2)]; s1z=[s1n(3) s2n(3)]; %4 LINEAS DE LA BASE b1x=[b1n(1) b2n(1)];%Lineas de la base b1y=[b1n(2) b2n(2)]; b1z=[b1n(3) b2n(3)];

b2x=[b2n(1) b3n(1)]; b2y=[b2n(2) b3n(2)]; b2z=[b2n(3) b3n(3)];



% Marco e: MARCO DEL MUNDO DE MATLAB O ESTACION DE TRABAJO e1x=[0 5]; e1y=[0 0]; e1z=[0 0];

e2x=[0 0]; e2y=[0 5];

e2z=[0 0];

e3x=[0 0]; e3y=[0 0]; e3z=[0 5];

%Matriz de transformacion del marco cero a la herramienta %T0h=T01*T12*T23*T34*T45*T56*T6h T0h = [ s6*(c4*s1 + s4*(c1*s2*s3 - c1*c2*c3)) + c6*(c5*(s1*s4 -

c4*(c1*s2*s3 - c1*c2*c3)) - s5*(c1*c2*s3 + c1*c3*s2)), c6*(c4*s1 +

s4*(c1*s2*s3 - c1*c2*c3)) - s6*(c5*(s1*s4 - c4*(c1*s2*s3 - c1*c2*c3)) -

s5*(c1*c2*s3 + c1*c3*s2)), s5*(s1*s4 - c4*(c1*s2*s3 - c1*c2*c3)) +

c5*(c1*c2*s3 + c1*c3*s2), r6*(s5*(s1*s4 - c4*(c1*s2*s3 - c1*c2*c3)) +

c5*(c1*c2*s3 + c1*c3*s2)) + c1*(d2 + d4*c23 + r4*s23 + d3*c2); - s6*(c1*c4 - s4*(s1*s2*s3 - c2*c3*s1)) - c6*(c5*(c1*s4 + c4*(s1*s2*s3 -

c2*c3*s1)) + s5*(c2*s1*s3 + c3*s1*s2)), s6*(c5*(c1*s4 + c4*(s1*s2*s3 -

c2*c3*s1)) + s5*(c2*s1*s3 + c3*s1*s2)) - c6*(c1*c4 - s4*(s1*s2*s3 -

c2*c3*s1)), c5*(c2*s1*s3 + c3*s1*s2) - s5*(c1*s4 + c4*(s1*s2*s3 -

c2*c3*s1)), s1*(d2 + d4*c23 + r4*s23 + d3*c2) - r6*(s5*(c1*s4 +

c4*(s1*s2*s3 - c2*c3*s1)) - c5*(c2*s1*s3 + c3*s1*s2)); c6*(s5*(c2*c3 - s2*s3) + c4*c5*(c2*s3 + c3*s2)) - s4*s6*(c2*s3 + c3*s2),

- s6*(s5*(c2*c3 - s2*s3) + c4*c5*(c2*s3 + c3*s2)) - c6*s4*(c2*s3 +

c3*s2), c4*s5*(c2*s3 + c3*s2) - c5*(c2*c3 - s2*s3), d4*s23 - r4*c23 +

d3*s2 + (r6*s23*s45)/2 - r6*c23*c5 - (r6*s54*s23)/2; 0, 0, 0, 1];

p1 = [4 8 0 1]';%Se describen 8 puntos de la pinza z1 = Te0*T0h*p1;

p2 = [4 8 8 1]'; z2 = Te0*T0h*p2;

p3 = [-4 8 0 1]'; z3 = Te0*T0h*p3;

p4 = [-4 8 8 1]'; z4 = Te0*T0h*p4;

p5 = [4 -8 0 1]'; z5 = Te0*T0h*p5;

p6 = [4 -8 8 1]'; z6 = Te0*T0h*p6;

p7 = [-4 -8 0 1]'; z7 = Te0*T0h*p7;

p8 = [-4 -8 8 1]'; z8 = Te0*T0h*p8;

%PARA DIBUJAR 10 LINEAS DE LA PINZA d12x=[z1(1) z2(1)]; d12y=[z1(2) z2(2)]; d12z=[z1(3) z2(3)]; d24x=[z2(1) z4(1)];

d24y=[z2(2) z4(2)]; d24z=[z2(3) z4(3)]; d13x=[z1(1) z3(1)]; d13y=[z1(2) z3(2)]; d13z=[z1(3) z3(3)]; d34x=[z3(1) z4(1)]; d34y=[z3(2) z4(2)]; d34z=[z3(3) z4(3)]; d15x=[z1(1) z5(1)]; d15y=[z1(2) z5(2)]; d15z=[z1(3) z5(3)]; d37x=[z3(1) z7(1)]; d37y=[z3(2) z7(2)]; d37z=[z3(3) z7(3)]; d57x=[z5(1) z7(1)]; d57y=[z5(2) z7(2)]; d57z=[z5(3) z7(3)]; d78x=[z7(1) z8(1)]; d78y=[z7(2) z8(2)]; d78z=[z7(3) z8(3)]; d86x=[z8(1) z6(1)]; d86y=[z8(2) z6(2)]; d86z=[z8(3) z6(3)]; d65x=[z6(1) z5(1)]; d65y=[z6(2) z5(2)]; d65z=[z6(3) z5(3)];

figure(1) clf % Esta instrucción limpia el espacio de graficación %hold on % Graficación de la ruta del OT: for j=2:i %j=2 para que existan 2 iteraciones realizadas plot3([pn1(j-1),pn1(j)],[pn2(j-1),pn2(j)],[pn3(j-1),pn3(j)]); hold on% Esta instrucción permite retener la traza de la ruta % En cada iteración, la ruta se dibuja desde el primer % punto hasta el actual end

% Graficación del robot en la configuración actual (iteración i):

grid on plot3(e1x,e1y,e1z,'m',e2x,e2y,e2z,'g',e3x,e3y,e3z,'c') plot3(b1x,b1y,b1z,'r',b2x,b2y,b2z,'r',b3x,b3y,b3z,'r',b4x,b4y,b4z,'r') plot3(s1x,s1y,s1z,'r','MarkerSize',5) plot3(r2x,r2y,r2z,'k',r3x,r3y,r3z,'k-

o',rax,ray,raz,'k',r4x,r4y,r4z,'k',r4ax,r4ay,r4az,'k-o',rhx,rhy,rhz,'k')

plot3(d12x,d12y,d12z,d24x,d24y,d24z,d13x,d13y,d13z,d34x,d34y,d34z,d15x,d1

5y,d15z,d37x,d37y,d37z,d57x,d57y,d57z,d78x,d78y,d78z,d86x,d86y,d86z,d65x,

d65y,d65z)

axis([-176,160,-170,200,-12,200]) view([1,1,1])

end

6- Efectuar la simulación para las siguientes trayectorias articulares:

𝜃𝑖 = 𝜃𝑖 𝑖𝑛𝑖+ 𝛿𝜃𝑖 ∙ 𝑓𝜃 𝑡 𝑝𝑎𝑟𝑎 𝑖 = 1, 2, ⋯ , 𝑛

Donde n = 6 (articulaciones) para el robot Motoman ES280D y donde además:

𝑓𝜃 𝑡 =𝑡

𝑇−

1

2 ∙ 𝜋∙ sin

2 ∙ π ∙ t

T

𝜃𝑖 y 𝛿𝑖 se asignan dependiendo de los limites articulares del robot.

A continuación se presenta en la Figura 4 la configuración inicial que especifica el

fabricante para que a partir de la misma se midan los limites articulares especificados en la

Tabla 1.

Figura 4. Configuracion inicial de referencia para límites articulares.

En la Figura 4 anterior el ángulo de θ2 inicial es igual a 90, debido a que no se tomo la

configuración inicial del fabricante para la ubicación de los marcos de referencia de cada

eslabón y posteriormente para la obtención de los PDHM.

-100

0

100

-50

0

50

100

150

200

0

50

100

150

200

Trayectoria 1: 𝜃1 = 0, 𝜃2 = 90 y 𝛿𝜃1 = 180

Trayectoria 2: 𝜃1 = 0, 𝜃2 = 90 y 𝛿𝜃1 = −180

-100

0

100

200

-100

0

100

200

0

50

100

150

200

250

-100

0

100

-100

0

100

200

0

50

100

150

200



-100

0

100

-100

0

100

200

0

50

100

150

200

-100

0

100

-100

0

100

200

-50

0

50

100

150

200

En las trayectorias anteriores (3 y 4) se reflejó que el sentido del eje z del marco del eslabón

2 asignado en la Figura 3 no corresponde físicamente al del robot Motoman ES280D,

debido a que cuando se le da un ángulo positivo en la articulación 2 el robot en la

simulación gira en sentido contrario. Esto se arregla reasignando los marcos de referencia

de cada eslabón del robot para que los ejes z de cada articulación correspondan en sentido

de giro a los ángulos que el fabricante especifica en la Figura 1.


Trayectoria 6: 𝜃3 = 0, 𝜃2 = 90 y 𝛿𝜃3 = −142.5

-100

0

100

-100

0

100

200

-50

0

50

100

150

200

-100

0

100

-100

0

100

200

-50

0

50

100

150

200



-100

0

100

-100

0

100

200

-50

0

50

100

150

200

-100

-50

0

50

100

-50

0

50

100

150

200

-50

0

50

100

150



-100

-50

0

50

100

-50

0

50

100

150

200

-50

0

50

100

150

-100

-50

0

50

100

-50

0

50

100

150

200

-50

0

50

100

150

REFERENCIAS BIBLIOGRÁFICAS

Robótica: Introducción al modelado de manipuladores.

Autor: J. Alfonso Pámanes García.

reporte de tarea 1 del mcdp para motoman es280d

Documents