tp2

tp2

April 17, 2015

1 Martn Nobla

1.1 Tp2

1.2 Control de Robots 2013

1.2.1 Ingeniera en Automatizacion y Control

1.2.2 Universidad Nacional de Quilmes

1.3 Ejercicio 1

Determinar los parametros de links(ai1, i1, di, i) y la cinematica (03T ) del manipulador detres brazos planar (3R) de la figura siguiente:

In [1]: from IPython.core.display import Image

In [2]: Image(filename=Imagenes/copy_left.png)

Out[2]:

In [3]: Image(filename=Imagenes/robot1_tp2_300.png)

Out[3]:

1

In [4]: from sympy import *

import numpy as np

#%pylab inline

In [5]: #Con esto las salidas van a ser en LaTeX

init_printing(use_latex=True)

Primero vamos a generar una funcion que genere la transformacion mas general para un vinculo, o sea:i1i T = RX(i1)DX(ai1)RZ(i)DZ(di)

In [6]: #Funcion simbolica para una rotacion(transformacion homogenea) sobre el eje X

def Rot_X(angle):

rad = angle*pi/180

M = Matrix([[1,0,0,0],[ 0,cos(rad),-sin(rad),0],[0,sin(rad), cos(rad),0],[0,0,0,1]])

return M

#Funcion simbolica para una rotacion(transformacion homogenea) sobre el eje Y

def Rot_Y(angle):

rad = angle*pi/180

M = Matrix([[cos(rad),0,sin(rad),0],[ 0,1,0,0],[-sin(rad), 0,cos(rad),0],[0,0,0,1]])

return M

#Funcion simbolica para una rotacion(transformacion homogenea) sobre el eje Z

def Rot_Z(angle):

rad = angle*pi/180

M = Matrix([[cos(rad),- sin(rad),0,0],[ sin(rad), cos(rad), 0,0],[0,0,1,0],[0,0,0,1]])

return M

2

In [7]: #Funcion simbolica para una traslacion en el eje X

def Traslacion_X(num):

D = Matrix([[1,0,0,num],[0,1,0,0],[0,0,1,0],[0,0,0,1]])

return D

#Funcion simbolica para una traslacion en el eje Y

def Traslacion_Y(num):

D = Matrix([[1,0,0,0],[0,1,0,num],[0,0,1,0],[0,0,0,1]])

return D

#Funcion simbolica para una traslacion en el eje Z

def Traslacion_Z(num):

D = Matrix([[1,0,0,0],[0,1,0,0],[0,0,1,num],[0,0,0,1]])

return D

In [8]: #estos son simbolos especiales que los toma como letras griegas directamente(muuy groso)

alpha, beta , gamma, phi, theta, a, d =symbols(alpha beta gamma phi theta a d)

In [9]: #Generamos la transformacion

T = Rot_X(alpha) * Traslacion_X(a) * Rot_Z(theta) * Traslacion_Z(d)

T

Out[9]: cos(pi180

) sin ( pi180) 0 asin(pi180

)cos(pi180

)cos(pi180

)cos(pi180

) sin ( pi180) d sin ( pi180)sin(pi180

)sin(pi180

)sin(pi180

)cos(pi180

)cos(pi180

)d cos

(pi180

)0 0 0 1

La tabla correspondiente con los parametros de Denavit-Hartemberg

i i1 ai1 di i

1 0 0 0 1

2 0 L1 0 2

3 0 L2 0 3

Ahora reemplazamos los parametros en la transformacion generica T para obtener cada transformacionde vnculo

In [10]: #Creamos los nuevos simbolos

theta_1, theta_2, theta_3, L_1, L_2 =symbols(theta_1, theta_2, theta_3, L_1, L_2)

In [11]: T_0_1 = T.subs([(alpha,0),(a,0),(d,0),(theta,theta_1)])

T_0_1

Out[11]: cos(pi1180

) sin (pi1180 ) 0 0sin(pi1180

)cos(pi1180

)0 0

0 0 1 00 0 0 1

In [12]: T_1_2 = T.subs([(alpha,0),(a,L_1),(d,0),(theta,theta_2)])

T_1_2

3

Out[12]: cos(pi2180

) sin (pi2180 ) 0 L1sin(pi2180

)cos(pi2180

)0 0

0 0 1 00 0 0 1


T_2_3

Out[13]: cos(pi3180

) sin (pi3180 ) 0 L2sin(pi3180

)cos(pi3180

)0 0

0 0 1 00 0 0 1

Y ahora finalmente calculamos la transformacion de la base a la herramienta como: 03T = (

01T )(

12T )(

23T )

In [14]: T_0_3 = T_0_1 * T_1_2 * T_2_3

T_0_3

Out[14]:

( sin (pi1180 ) sin (pi2180 )+ cos (pi1180 ) cos (pi2180 )) cos (pi3180 )+ ( sin (pi1180 ) cos (pi2180 ) sin (pi2180 ) cos (pi1180 )) sin (pi3180 ) ( sin (pi1180 ) sin (pi2180 )+ cos (pi1180 ) cos (pi2180 )) sin (pi3180 )+ ( sin (pi1180 ) cos (pi2180 ) sin (pi2180 ) cos (pi1180 )) cos (pi3180 ) 0 L1 cos (pi1180 )+ L2 ( sin (pi1180 ) sin (pi2180 )+ cos (pi1180 ) cos (pi2180 ))( sin (pi1180 ) sin (pi2180 )+ cos (pi1180 ) cos (pi2180 )) sin (pi3180 )+ (sin (pi1180 ) cos (pi2180 )+ sin (pi2180 ) cos (pi1180 )) cos (pi3180 ) ( sin (pi1180 ) sin (pi2180 )+ cos (pi1180 ) cos (pi2180 )) cos (pi3180 ) (sin (pi1180 ) cos (pi2180 )+ sin (pi2180 ) cos (pi1180 )) sin (pi3180 ) 0 L1 sin (pi1180 )+ L2 (sin (pi1180 ) cos (pi2180 )+ sin (pi2180 ) cos (pi1180 ))

0 0 1 00 0 0 1

In [15]: #Verificamos asignando los angulos a cero, nos debera dar la suma de las L en X

T_cero = T_0_3.subs([(theta_1,0),(theta_2,0),(theta_3,0)])

T_cero

Out[15]: 1 0 0 L1 + L20 1 0 00 0 1 00 0 0 1

1.4 Ejercicio 2

El manipulador siguiente tiene 3 grados de libertad similar al ejercicio anterior excepto queel eje de la primer junta no es paralelo a los otros dos. Determinar los parametros de links(ai1, i1, di,i )y la cinematica BWT . Notar que existe un desfasaje de 90

entre los ejes 1 y2, y que no es necesario definir L3.

In [16]: Image(filename=Imagenes/dibujo_robot2_tp2.png)

Out[16]:

4


i i1 ai1 di i

1 0 0 0 1

2 90o L1 0 2

3 0 L2 0 3

Nuevamente utilizamos la transformacion generica y reemplazamos los valores de la tabla


T_0_1

Out[17]: cos(pi1180

) sin (pi1180 ) 0 0sin(pi1180

)cos(pi1180

)0 0

0 0 1 00 0 0 1


T_1_2

Out[18]: cos(pi2180

) sin (pi2180 ) 0 L10 0 1 0

sin(pi2180

)cos(pi2180

)0 0

0 0 0 1

5


T_2_3

Out[19]: cos(pi3180

) sin (pi3180 ) 0 L2sin(pi3180

)cos(pi3180

)0 0

0 0 1 00 0 0 1

In [20]: T_B_W = T_0_1 * T_1_2 * T_2_3

T_B_W.simplify()

T_B_W

Out[20]:

cos(pi1180

)cos(pi(2180 +

3180

)) sin (pi ( 2180 + 3180)) cos (pi1180 ) sin (pi1180 ) (L1 + L2 cos (pi2180 )) cos (pi1180 )sin(pi1180

)cos(pi(2180 +

3180

)) sin (pi1180 ) sin (pi ( 2180 + 3180)) cos (pi1180 ) (L1 + L2 cos (pi2180 )) sin (pi1180 )sin(pi(2180 +

3180

))cos(pi(2180 +

3180

))0 L2 sin

(pi2180

)0 0 0 1

In [21]: #Verificamos nuevamente

T_cero_2 = T_B_W.subs([(theta_1,0),(theta_2,0),(theta_3,0)])

T_cero_2

Out[21]: 1 0 0 L1 + L20 0 1 00 1 0 00 0 0 1

1.5 Ejercicio 3

El manipulador de 3 grados de libertad de la figura siguiente tiene las juntas 1 y 2 perpendic-ulares, y las juntas 2 y 3 paralelas. Segun el grafico, todas las juntas estan en posicion cero yademas se muestra el sentido de giro positivo de cada una. Asignar los frames correspondientespara luego hallar: (01T ), (

12T ), (

23T )

In [22]: Image(filename=Imagenes/robot3_tp2.png)

Out[22]:

6


i i1 ai1 di i

1 0 0 L1 + L2 1

2 90o 0 0 2

3 0 L3 0 3

4 0 L4 0 0


In [23]: T_0_1 = T.subs([(alpha,0),(a,0),(d,L_1+L_2),(theta,theta_1)])

T_0_1

Out[23]: cos(pi1180

) sin (pi1180 ) 0 0sin(pi1180

)cos(pi1180

)0 0

0 0 1 L1 + L20 0 0 1


T_1_2

7

Out[24]: cos(pi2180

) sin (pi2180 ) 0 00 0 1 0

sin(pi2180

)cos(pi2180

)0 0

0 0 0 1

In [25]: #creamos el simbolo que nos faltaba

L_3 = symbols(L_3)


T_2_3

Out[26]: cos(pi3180

) sin (pi3180 ) 0 L3sin(pi3180

)cos(pi3180

)0 0

0 0 1 00 0 0 1

1.6 Ejercicio 4

Para el manipular de 2 brazos siguiente, las matrices: (01T ), (12T ) fueron determinadas, y su

producto es: 20A. Notar que el frame {0} es coincidente con el {1} cuando 1 = 0. La longituddel segundo brazo es L2. Encontrar una expresion para el vector

0PTIP


Out[27]:

In [28]: A_0_2 = Matrix([[cos(theta_1)*cos(theta_2),-cos(theta_1)*sin(theta_2),sin(theta_1),L_1*cos(theta_1)],[sin(theta_1)*cos(theta_2),-sin(theta_1)*sin(theta_2),-cos(theta_1),L_1*sin(theta_1)],[sin(theta_2),cos(theta_2),0,0],[0,0,0,1]])

A_0_2

8

Out[28]: cos (1) cos (2) sin (2) cos (1) sin (1) L1 cos (1)sin (1) cos (2) sin (1) sin (2) cos (1) L1 sin (1)

sin (2) cos (2) 0 00 0 0 1

Podemos ver que 0PTIP = (

20A

2)PTIP , entonces2PTIP = [L2, 0, 0]

T

In [29]: P_TIP = Matrix([[L_2,0,0,1]])

P_TIP = P_TIP.T

P_TIP

Out[29]: L2001

In [30]: #Hallamos el vector buscado

P_TIP_0 = A_0_2 * P_TIP

P_TIP_0

Out[30]: L1 cos (1) + L2 cos (1) cos (2)L1 sin (1) + L2 sin (1) cos (2)

L2 sin (2)1

1.7 Ejercicio 5

Determinar los parametros de links (ai1, i1, di, i) y la cinematica (03A) del manipulador de3 brazos RPP de la figura siguiente:


Out[31]:

9


i i1 ai1 di i

1 0 0 0 1

2 90 d2 0 0

3 0 d3 0 0



T_0_1

Out[32]: cos(pi1180

) sin (pi1180 ) 0 0sin(pi1180

)cos(pi1180

)0 0

0 0 1 00 0 0 1

In [33]: d_3, d_2 = symbols(d_3 d_2)

In [34]: T_1_2 = T.subs([(alpha,90),(a,d_2),(d,0),(theta,0)])

T_1_2

10

Out[34]: 1 0 0 d20 0 1 00 1 0 00 0 0 1

In [35]: T_2_3 = T.subs([(alpha,0),(a,d_3),(d,0),(theta,0)])

T_2_3

Out[35]: 1 0 0 d30 1 0 00 0 1 00 0 0 1

In [36]: #Obtenemos la transformacion final

T_0_3 = T_0_1 * T_1_2 * T_2_3

T_0_3

Out[36]: cos(pi1180

)0 sin

(pi1180

)d2 cos

(pi1180

)+ d3 cos

(pi1180

)sin(pi1180

)0 cos (pi1180 ) d2 sin (pi1180 )+ d3 sin (pi1180 )

0 1 0 00 0 0 1

In [ ]:

11

Martn NoblaTp2Control de Robots 2013Ingeniera en Automatizacin y ControlUniversidad Nacional de Quilmes

Ejercicio 1Ejercicio 2Ejercicio 3Ejercicio 4Ejercicio 5

tp2

Documents