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]:
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 función que genere la transformación más general para un vinculo, o sea:
$^{i-1}_{i} T = R_{X}(\alpha_{i-1})D_{X}(a_{i-1})R_{Z}(\theta_{i})D_{Z}(d_{i})$
In [6]:
#Funcion simbólica para una rotación(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 simbólica para una rotación(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 simbólica para una rotación(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
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]:
La tabla correspondiente con los parámetros de Denavit-Hartemberg
| $i$ | $\alpha_{i-1}$ | $a_{i-1}$ | $d_{i}$ | $\theta_{i}$ |
|---|---|---|---|---|
| 1 | 0 | 0 | 0 | $\theta_{1}$ |
| 2 | 0 | $L_{1}$ | 0 | $\theta_{2}$ |
| 3 | 0 | $L_{2}$ | 0 | $\theta_{3}$ |
Ahora reemplazamos los parámetros en la transformación genérica $T$ para obtener cada transformación de vínculo
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]:
In [12]:
T_1_2 = T.subs([(alpha,0),(a,L_1),(d,0),(theta,theta_2)])
T_1_2
Out[12]:
In [13]:
T_2_3 = T.subs([(alpha,0),(a,L_2),(d,0),(theta,theta_3)])
T_2_3
Out[13]:
Y ahora finalmente calculamos la transformación de la base a la herramienta como: $^{0}_{3} T = (^{0}_{1} T) (^{1}_{2} T) (^{2}_{3} T) $
In [14]:
T_0_3 = T_0_1 * T_1_2 * T_2_3
T_0_3
Out[14]:
In [15]:
#Verificamos asignando los angulos a cero, nos debería 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]:
In [16]:
Image(filename='Imagenes/dibujo_robot2_tp2.png')
Out[16]:
La tabla correspondiente con los parámetros de Denavit-Hartemberg
| $i$ | $\alpha_{i-1}$ | $a_{i-1}$ | $d_{i}$ | $\theta_{i}$ |
|---|---|---|---|---|
| 1 | 0 | 0 | 0 | $\theta_{1}$ |
| 2 | 90º | $L_{1}$ | 0 | $\theta_{2}$ |
| 3 | 0 | $L_{2}$ | 0 | $\theta_{3}$ |
Nuevamente utilizamos la transformación 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]:
In [18]:
T_1_2 = T.subs([(alpha,90),(a,L_1),(d,0),(theta,theta_2)])
T_1_2
Out[18]:
In [19]:
T_2_3 = T.subs([(alpha,0),(a,L_2),(d,0),(theta,theta_3)])
T_2_3
Out[19]:
In [20]:
T_B_W = T_0_1 * T_1_2 * T_2_3
T_B_W.simplify()
T_B_W
Out[20]:
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]:
$(^{0}_{1} T), (^{1}_{2} T), (^{2}_{3} T) $
In [22]:
Image(filename='Imagenes/robot3_tp2.png')
Out[22]:
La tabla correspondiente con los parámetros de Denavit-Hartemberg
| $i$ | $\alpha_{i-1}$ | $a_{i-1}$ | $d_{i}$ | $\theta_{i}$ |
|---|---|---|---|---|
| 1 | 0 | 0 | $L_{1}+L_{2}$ | $\theta_{1}$ |
| 2 | 90º | 0 | 0 | $\theta_{2}$ |
| 3 | 0 | $L_{3}$ | 0 | $\theta_{3}$ |
| 4 | 0 | $L_{4}$ | 0 | 0 |
Nuevamente utilizamos la transformación 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]:
In [24]:
T_1_2 = T.subs([(alpha,90),(a,0),(d,0),(theta,theta_2)])
T_1_2
Out[24]:
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]:
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
Out[28]:
Podemos ver que $^{0}P_{TIP}=(^{2}_{0}A^{2})P_{TIP}$, entonces $^{2}P_{TIP}=[L_2,0,0]^{T}$
In [29]:
P_TIP = Matrix([[L_2,0,0,1]])
P_TIP = P_TIP.T
P_TIP
Out[29]:
In [30]:
#Hallamos el vector buscado
P_TIP_0 = A_0_2 * P_TIP
P_TIP_0
Out[30]:
In [31]:
Image(filename='Imagenes/robot5_tp2.png')
Out[31]:
La tabla correspondiente con los parámetros de Denavit-Hartemberg
| $i$ | $\alpha_{i-1}$ | $a_{i-1}$ | $d_{i}$ | $\theta_{i}$ |
|---|---|---|---|---|
| 1 | 0 | 0 | 0 | $\theta_{1}$ |
| 2 | 90 | $d_{2}$ | 0 | 0 |
| 3 | 0 | $d_{3}$ | 0 | 0 |
Nuevamente utilizamos la transformación 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]:
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
Out[34]:
In [35]:
T_2_3 = T.subs([(alpha,0),(a,d_3),(d,0),(theta,0)])
T_2_3
Out[35]:
In [36]:
#Obtenemos la transformacion final
T_0_3 = T_0_1 * T_1_2 * T_2_3
T_0_3
Out[36]:
In [36]: