Martín Noblía

Tp2

Control de Robots 2013

Ingeniería en Automatización y Control

Universidad Nacional de Quilmes

Ejercicio 1

Determinar los parámetros de links($a_{i-1}$, $\alpha_{i-1}$, $d_{i}$, $\theta_{i}$) y la cinemática ($^{0}_{3}T$) del manipulador de tres 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]:

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]:
$$\left[\begin{smallmatrix}\cos{\left (\frac{1}{180} \pi \theta \right )} & - \sin{\left (\frac{1}{180} \pi \theta \right )} & 0 & a\\\sin{\left (\frac{1}{180} \pi \theta \right )} \cos{\left (\frac{1}{180} \pi \alpha \right )} & \cos{\left (\frac{1}{180} \pi \alpha \right )} \cos{\left (\frac{1}{180} \pi \theta \right )} & - \sin{\left (\frac{1}{180} \pi \alpha \right )} & - d \sin{\left (\frac{1}{180} \pi \alpha \right )}\\\sin{\left (\frac{1}{180} \pi \alpha \right )} \sin{\left (\frac{1}{180} \pi \theta \right )} & \sin{\left (\frac{1}{180} \pi \alpha \right )} \cos{\left (\frac{1}{180} \pi \theta \right )} & \cos{\left (\frac{1}{180} \pi \alpha \right )} & d \cos{\left (\frac{1}{180} \pi \alpha \right )}\\0 & 0 & 0 & 1\end{smallmatrix}\right]$$

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]:
$$\left[\begin{smallmatrix}\cos{\left (\frac{1}{180} \pi \theta_{1} \right )} & - \sin{\left (\frac{1}{180} \pi \theta_{1} \right )} & 0 & 0\\\sin{\left (\frac{1}{180} \pi \theta_{1} \right )} & \cos{\left (\frac{1}{180} \pi \theta_{1} \right )} & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{smallmatrix}\right]$$

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


Out[12]:
$$\left[\begin{smallmatrix}\cos{\left (\frac{1}{180} \pi \theta_{2} \right )} & - \sin{\left (\frac{1}{180} \pi \theta_{2} \right )} & 0 & L_{1}\\\sin{\left (\frac{1}{180} \pi \theta_{2} \right )} & \cos{\left (\frac{1}{180} \pi \theta_{2} \right )} & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{smallmatrix}\right]$$

In [13]:
T_2_3 = T.subs([(alpha,0),(a,L_2),(d,0),(theta,theta_3)])
T_2_3


Out[13]:
$$\left[\begin{smallmatrix}\cos{\left (\frac{1}{180} \pi \theta_{3} \right )} & - \sin{\left (\frac{1}{180} \pi \theta_{3} \right )} & 0 & L_{2}\\\sin{\left (\frac{1}{180} \pi \theta_{3} \right )} & \cos{\left (\frac{1}{180} \pi \theta_{3} \right )} & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{smallmatrix}\right]$$

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]:
$$\left[\begin{smallmatrix}\left(- \sin{\left (\frac{1}{180} \pi \theta_{1} \right )} \sin{\left (\frac{1}{180} \pi \theta_{2} \right )} + \cos{\left (\frac{1}{180} \pi \theta_{1} \right )} \cos{\left (\frac{1}{180} \pi \theta_{2} \right )}\right) \cos{\left (\frac{1}{180} \pi \theta_{3} \right )} + \left(- \sin{\left (\frac{1}{180} \pi \theta_{1} \right )} \cos{\left (\frac{1}{180} \pi \theta_{2} \right )} - \sin{\left (\frac{1}{180} \pi \theta_{2} \right )} \cos{\left (\frac{1}{180} \pi \theta_{1} \right )}\right) \sin{\left (\frac{1}{180} \pi \theta_{3} \right )} & - \left(- \sin{\left (\frac{1}{180} \pi \theta_{1} \right )} \sin{\left (\frac{1}{180} \pi \theta_{2} \right )} + \cos{\left (\frac{1}{180} \pi \theta_{1} \right )} \cos{\left (\frac{1}{180} \pi \theta_{2} \right )}\right) \sin{\left (\frac{1}{180} \pi \theta_{3} \right )} + \left(- \sin{\left (\frac{1}{180} \pi \theta_{1} \right )} \cos{\left (\frac{1}{180} \pi \theta_{2} \right )} - \sin{\left (\frac{1}{180} \pi \theta_{2} \right )} \cos{\left (\frac{1}{180} \pi \theta_{1} \right )}\right) \cos{\left (\frac{1}{180} \pi \theta_{3} \right )} & 0 & L_{1} \cos{\left (\frac{1}{180} \pi \theta_{1} \right )} + L_{2} \left(- \sin{\left (\frac{1}{180} \pi \theta_{1} \right )} \sin{\left (\frac{1}{180} \pi \theta_{2} \right )} + \cos{\left (\frac{1}{180} \pi \theta_{1} \right )} \cos{\left (\frac{1}{180} \pi \theta_{2} \right )}\right)\\\left(- \sin{\left (\frac{1}{180} \pi \theta_{1} \right )} \sin{\left (\frac{1}{180} \pi \theta_{2} \right )} + \cos{\left (\frac{1}{180} \pi \theta_{1} \right )} \cos{\left (\frac{1}{180} \pi \theta_{2} \right )}\right) \sin{\left (\frac{1}{180} \pi \theta_{3} \right )} + \left(\sin{\left (\frac{1}{180} \pi \theta_{1} \right )} \cos{\left (\frac{1}{180} \pi \theta_{2} \right )} + \sin{\left (\frac{1}{180} \pi \theta_{2} \right )} \cos{\left (\frac{1}{180} \pi \theta_{1} \right )}\right) \cos{\left (\frac{1}{180} \pi \theta_{3} \right )} & \left(- \sin{\left (\frac{1}{180} \pi \theta_{1} \right )} \sin{\left (\frac{1}{180} \pi \theta_{2} \right )} + \cos{\left (\frac{1}{180} \pi \theta_{1} \right )} \cos{\left (\frac{1}{180} \pi \theta_{2} \right )}\right) \cos{\left (\frac{1}{180} \pi \theta_{3} \right )} - \left(\sin{\left (\frac{1}{180} \pi \theta_{1} \right )} \cos{\left (\frac{1}{180} \pi \theta_{2} \right )} + \sin{\left (\frac{1}{180} \pi \theta_{2} \right )} \cos{\left (\frac{1}{180} \pi \theta_{1} \right )}\right) \sin{\left (\frac{1}{180} \pi \theta_{3} \right )} & 0 & L_{1} \sin{\left (\frac{1}{180} \pi \theta_{1} \right )} + L_{2} \left(\sin{\left (\frac{1}{180} \pi \theta_{1} \right )} \cos{\left (\frac{1}{180} \pi \theta_{2} \right )} + \sin{\left (\frac{1}{180} \pi \theta_{2} \right )} \cos{\left (\frac{1}{180} \pi \theta_{1} \right )}\right)\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{smallmatrix}\right]$$

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]:
$$\left[\begin{smallmatrix}1 & 0 & 0 & L_{1} + L_{2}\\0 & 1 & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{smallmatrix}\right]$$

Ejercicio 2


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]:
$$\left[\begin{smallmatrix}\cos{\left (\frac{1}{180} \pi \theta_{1} \right )} & - \sin{\left (\frac{1}{180} \pi \theta_{1} \right )} & 0 & 0\\\sin{\left (\frac{1}{180} \pi \theta_{1} \right )} & \cos{\left (\frac{1}{180} \pi \theta_{1} \right )} & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{smallmatrix}\right]$$

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


Out[18]:
$$\left[\begin{smallmatrix}\cos{\left (\frac{1}{180} \pi \theta_{2} \right )} & - \sin{\left (\frac{1}{180} \pi \theta_{2} \right )} & 0 & L_{1}\\0 & 0 & -1 & 0\\\sin{\left (\frac{1}{180} \pi \theta_{2} \right )} & \cos{\left (\frac{1}{180} \pi \theta_{2} \right )} & 0 & 0\\0 & 0 & 0 & 1\end{smallmatrix}\right]$$

In [19]:
T_2_3 = T.subs([(alpha,0),(a,L_2),(d,0),(theta,theta_3)])
T_2_3


Out[19]:
$$\left[\begin{smallmatrix}\cos{\left (\frac{1}{180} \pi \theta_{3} \right )} & - \sin{\left (\frac{1}{180} \pi \theta_{3} \right )} & 0 & L_{2}\\\sin{\left (\frac{1}{180} \pi \theta_{3} \right )} & \cos{\left (\frac{1}{180} \pi \theta_{3} \right )} & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{smallmatrix}\right]$$

In [20]:
T_B_W = T_0_1 * T_1_2 * T_2_3
T_B_W.simplify()
T_B_W


Out[20]:
$$\left[\begin{smallmatrix}\left(- \sin{\left (\frac{1}{180} \pi \theta_{2} \right )} \sin{\left (\frac{1}{180} \pi \theta_{3} \right )} + \cos{\left (\frac{1}{180} \pi \theta_{2} \right )} \cos{\left (\frac{1}{180} \pi \theta_{3} \right )}\right) \cos{\left (\frac{1}{180} \pi \theta_{1} \right )} & \left(- \sin{\left (\frac{1}{180} \pi \theta_{2} \right )} \cos{\left (\frac{1}{180} \pi \theta_{3} \right )} - \sin{\left (\frac{1}{180} \pi \theta_{3} \right )} \cos{\left (\frac{1}{180} \pi \theta_{2} \right )}\right) \cos{\left (\frac{1}{180} \pi \theta_{1} \right )} & \sin{\left (\frac{1}{180} \pi \theta_{1} \right )} & \left(L_{1} + L_{2} \cos{\left (\frac{1}{180} \pi \theta_{2} \right )}\right) \cos{\left (\frac{1}{180} \pi \theta_{1} \right )}\\\left(- \sin{\left (\frac{1}{180} \pi \theta_{2} \right )} \sin{\left (\frac{1}{180} \pi \theta_{3} \right )} + \cos{\left (\frac{1}{180} \pi \theta_{2} \right )} \cos{\left (\frac{1}{180} \pi \theta_{3} \right )}\right) \sin{\left (\frac{1}{180} \pi \theta_{1} \right )} & \left(- \sin{\left (\frac{1}{180} \pi \theta_{2} \right )} \cos{\left (\frac{1}{180} \pi \theta_{3} \right )} - \sin{\left (\frac{1}{180} \pi \theta_{3} \right )} \cos{\left (\frac{1}{180} \pi \theta_{2} \right )}\right) \sin{\left (\frac{1}{180} \pi \theta_{1} \right )} & - \cos{\left (\frac{1}{180} \pi \theta_{1} \right )} & \left(L_{1} + L_{2} \cos{\left (\frac{1}{180} \pi \theta_{2} \right )}\right) \sin{\left (\frac{1}{180} \pi \theta_{1} \right )}\\\sin{\left (\frac{1}{180} \pi \theta_{2} \right )} \cos{\left (\frac{1}{180} \pi \theta_{3} \right )} + \sin{\left (\frac{1}{180} \pi \theta_{3} \right )} \cos{\left (\frac{1}{180} \pi \theta_{2} \right )} & - \sin{\left (\frac{1}{180} \pi \theta_{2} \right )} \sin{\left (\frac{1}{180} \pi \theta_{3} \right )} + \cos{\left (\frac{1}{180} \pi \theta_{2} \right )} \cos{\left (\frac{1}{180} \pi \theta_{3} \right )} & 0 & L_{2} \sin{\left (\frac{1}{180} \pi \theta_{2} \right )}\\0 & 0 & 0 & 1\end{smallmatrix}\right]$$

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]:
$$\left[\begin{smallmatrix}1 & 0 & 0 & L_{1} + L_{2}\\0 & 0 & -1 & 0\\0 & 1 & 0 & 0\\0 & 0 & 0 & 1\end{smallmatrix}\right]$$

Ejercicio 3

El manipulador de 3 grados de libertad de la figura siguiente tiene las juntas 1 y 2 perpendiculares, y las juntas 2 y 3 paralelas. Según el gráfico, todas las juntas están en posición cero y además se muestra el sentido de giro positivo de cada una. Asignar los frames correspondientes para luego hallar:

$(^{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]:
$$\left[\begin{smallmatrix}\cos{\left (\frac{1}{180} \pi \theta_{1} \right )} & - \sin{\left (\frac{1}{180} \pi \theta_{1} \right )} & 0 & 0\\\sin{\left (\frac{1}{180} \pi \theta_{1} \right )} & \cos{\left (\frac{1}{180} \pi \theta_{1} \right )} & 0 & 0\\0 & 0 & 1 & L_{1} + L_{2}\\0 & 0 & 0 & 1\end{smallmatrix}\right]$$

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


Out[24]:
$$\left[\begin{smallmatrix}\cos{\left (\frac{1}{180} \pi \theta_{2} \right )} & - \sin{\left (\frac{1}{180} \pi \theta_{2} \right )} & 0 & 0\\0 & 0 & -1 & 0\\\sin{\left (\frac{1}{180} \pi \theta_{2} \right )} & \cos{\left (\frac{1}{180} \pi \theta_{2} \right )} & 0 & 0\\0 & 0 & 0 & 1\end{smallmatrix}\right]$$

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]:
$$\left[\begin{smallmatrix}\cos{\left (\frac{1}{180} \pi \theta_{3} \right )} & - \sin{\left (\frac{1}{180} \pi \theta_{3} \right )} & 0 & L_{3}\\\sin{\left (\frac{1}{180} \pi \theta_{3} \right )} & \cos{\left (\frac{1}{180} \pi \theta_{3} \right )} & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{smallmatrix}\right]$$

Ejercicio 4

Para el manipular de 2 brazos siguiente, las matrices: $(^{0}_{1} T), (^{1}_{2} T)$ fueron determinadas, y su producto es: $^{2}_{0}A$. Notar que el frame {0} es coincidente con el {1} cuando $\theta_{1}=0$. La longitud del segundo brazo es $L_2$. Encontrar una expresión para el vector $^{0}P_{TIP}$


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]:
$$\left[\begin{smallmatrix}\cos{\left (\theta_{1} \right )} \cos{\left (\theta_{2} \right )} & - \sin{\left (\theta_{2} \right )} \cos{\left (\theta_{1} \right )} & \sin{\left (\theta_{1} \right )} & L_{1} \cos{\left (\theta_{1} \right )}\\\sin{\left (\theta_{1} \right )} \cos{\left (\theta_{2} \right )} & - \sin{\left (\theta_{1} \right )} \sin{\left (\theta_{2} \right )} & - \cos{\left (\theta_{1} \right )} & L_{1} \sin{\left (\theta_{1} \right )}\\\sin{\left (\theta_{2} \right )} & \cos{\left (\theta_{2} \right )} & 0 & 0\\0 & 0 & 0 & 1\end{smallmatrix}\right]$$

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]:
$$\left[\begin{smallmatrix}L_{2}\\0\\0\\1\end{smallmatrix}\right]$$

In [30]:
#Hallamos el vector buscado
P_TIP_0 = A_0_2 * P_TIP
P_TIP_0


Out[30]:
$$\left[\begin{smallmatrix}L_{1} \cos{\left (\theta_{1} \right )} + L_{2} \cos{\left (\theta_{1} \right )} \cos{\left (\theta_{2} \right )}\\L_{1} \sin{\left (\theta_{1} \right )} + L_{2} \sin{\left (\theta_{1} \right )} \cos{\left (\theta_{2} \right )}\\L_{2} \sin{\left (\theta_{2} \right )}\\1\end{smallmatrix}\right]$$

Ejercicio 5


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]:
$$\left[\begin{smallmatrix}\cos{\left (\frac{1}{180} \pi \theta_{1} \right )} & - \sin{\left (\frac{1}{180} \pi \theta_{1} \right )} & 0 & 0\\\sin{\left (\frac{1}{180} \pi \theta_{1} \right )} & \cos{\left (\frac{1}{180} \pi \theta_{1} \right )} & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{smallmatrix}\right]$$

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]:
$$\left[\begin{smallmatrix}1 & 0 & 0 & d_{2}\\0 & 0 & -1 & 0\\0 & 1 & 0 & 0\\0 & 0 & 0 & 1\end{smallmatrix}\right]$$

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


Out[35]:
$$\left[\begin{smallmatrix}1 & 0 & 0 & d_{3}\\0 & 1 & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{smallmatrix}\right]$$

In [36]:
#Obtenemos la transformacion final
T_0_3 = T_0_1 * T_1_2 * T_2_3
T_0_3


Out[36]:
$$\left[\begin{smallmatrix}\cos{\left (\frac{1}{180} \pi \theta_{1} \right )} & 0 & \sin{\left (\frac{1}{180} \pi \theta_{1} \right )} & d_{2} \cos{\left (\frac{1}{180} \pi \theta_{1} \right )} + d_{3} \cos{\left (\frac{1}{180} \pi \theta_{1} \right )}\\\sin{\left (\frac{1}{180} \pi \theta_{1} \right )} & 0 & - \cos{\left (\frac{1}{180} \pi \theta_{1} \right )} & d_{2} \sin{\left (\frac{1}{180} \pi \theta_{1} \right )} + d_{3} \sin{\left (\frac{1}{180} \pi \theta_{1} \right )}\\0 & 1 & 0 & 0\\0 & 0 & 0 & 1\end{smallmatrix}\right]$$

In [36]: