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
In [13]: T_2_3 = T.subs([(alpha,0),(a,L_2),(d,0),(theta,theta_3)])
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
La tabla correspondiente con los parametros de Denavit-Hartemberg
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
In [17]: T_0_1 = T.subs([(alpha,0),(a,0),(d,0),(theta,theta_1)])
T_0_1
Out[17]: cos(pi1180
) sin (pi1180 ) 0 0sin(pi1180
)cos(pi1180
)0 0
0 0 1 00 0 0 1
In [18]: T_1_2 = T.subs([(alpha,90),(a,L_1),(d,0),(theta,theta_2)])
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
In [19]: T_2_3 = T.subs([(alpha,0),(a,L_2),(d,0),(theta,theta_3)])
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
La tabla correspondiente con los parametros de Denavit-Hartemberg
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
Nuevamente utilizamos la transformacion generica y reemplazamos los valores de la tabla
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
In [24]: T_1_2 = T.subs([(alpha,90),(a,0),(d,0),(theta,theta_2)])
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)
In [26]: T_2_3 = T.subs([(alpha,0),(a,L_3),(d,0),(theta,theta_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
In [27]: Image(filename=Imagenes/robot4_tp2.png)
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:
In [31]: Image(filename=Imagenes/robot5_tp2.png)
Out[31]:
9
La tabla correspondiente con los parametros de Denavit-Hartemberg
i i1 ai1 di i
1 0 0 0 1
2 90 d2 0 0
3 0 d3 0 0
Nuevamente utilizamos la transformacion generica y reemplazamos los valores de la tabla
In [32]: T_0_1 = T.subs([(alpha,0),(a,0),(d,0),(theta,theta_1)])
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
Recommended