<SYSTEM_TASK:>
Given the following text description, write Python code to implement the functionality described below step by step
<END_TASK>
<USER_TASK:>
Description:
 Problemas
Defina una función que tome como argumentos los parametros de Denavit - Hartenberg, y cree una matriz de transformación homogénea.
Step1:  Cree una función que tome como argumentos los parametros de los grados de libertad de un manipulador tipo PUMA y devuelva las matrices de transformación homogénea asociadas a cada eslabon.
Step2:  Cree una función que dados los angulos del manipulador devuelva la transformación total del manipulador (ayudese de la función creada en el segundo problema).
Step3:  Cree una función que dados los angulos del manipulador, grafique las posiciones de los eslabones del manipulador del primer punto (ayudese de las funciones creadas en el primer y segundo problemas, modificadas ligeramente para aceptar matrices numéricas, así como la función creada en la práctica anterior para la graficación de un sistema robótico).
Step4:  Utilice la función interact para manipular la posición del manipulador, de tal manera que su posición sea aproximadamente $q_1=0.6rad$, $q_2=0.2rad$ y $q_3 = -0.8rad$
<ASSISTANT_TASK:>
Python Code:
def DH_simbolico(a, d, α, θ):
    from sympy import Matrix, sin, cos
    # YOUR CODE HERE
    raise NotImplementedError()
from sympy import Matrix, sin, cos, pi
from nose.tools import assert_equal
assert_equal(DH_simbolico(0,0,0,pi/2), Matrix([[0,-1,0,0],[1,0,0,0], [0,0,1,0],[0,0,0,1]]))
assert_equal(DH_simbolico(0,0,pi/2,0), Matrix([[1,0,0,0],[0,0,-1,0], [0,1,0,0],[0,0,0,1]]))
assert_equal(DH_simbolico(0,1,0,0), Matrix([[1,0,0,0],[0,1,0,0], [0,0,1,1],[0,0,0,1]]))
assert_equal(DH_simbolico(1,0,0,0), Matrix([[1,0,0,1],[0,1,0,0], [0,0,1,0],[0,0,0,1]]))
Explanation: Problemas
Defina una función que tome como argumentos los parametros de Denavit - Hartenberg, y cree una matriz de transformación homogénea.
End of explanation
def cinematica_PUMA(q1, q2, q3):
    from sympy import pi, var
    var("l1:4")
    # YOUR CODE HERE
    raise NotImplementedError()
    return A1, A2, A3
from nose.tools import assert_equal
from sympy import pi, var, Matrix
var("l1:4")
A1, A2, A3 = cinematica_PUMA(0, 0, 0)
assert_equal(A1*A2*A3, Matrix([[1,0,0,l2+l3], [0,0,-1,0], [0,1,0,l1], [0,0,0,1]]))
A1, A2, A3 = cinematica_PUMA(pi/2, 0, 0)
assert_equal(A1*A2*A3, Matrix([[0,0,1,0], [1,0,0,l2+l3], [0,1,0,l1], [0,0,0,1]]))
A1, A2, A3 = cinematica_PUMA(0, pi/2, 0)
assert_equal(A1*A2*A3, Matrix([[0,-1,0,0], [0,0,-1,0], [1,0,0,l1+l2+l3], [0,0,0,1]]))
A1, A2, A3 = cinematica_PUMA(0, 0, pi/2)
assert_equal(A1*A2*A3, Matrix([[0,-1,0,l2], [0,0,-1,0], [1,0,0,l1+l3], [0,0,0,1]]))
Explanation: Cree una función que tome como argumentos los parametros de los grados de libertad de un manipulador tipo PUMA y devuelva las matrices de transformación homogénea asociadas a cada eslabon.
End of explanation
def transformacion_PUMA(q1, q2, q3):
    from sympy import pi, var
    var("l1:4")
    # YOUR CODE HERE
    raise NotImplementedError()
from nose.tools import assert_equal
from sympy import pi, var, Matrix
var("l1:4")
assert_equal(transformacion_PUMA(0, 0, 0), Matrix([[1,0,0,l2+l3], [0,0,-1,0], [0,1,0,l1], [0,0,0,1]]))
assert_equal(transformacion_PUMA(pi/2, 0, 0), Matrix([[0,0,1,0], [1,0,0,l2+l3], [0,1,0,l1], [0,0,0,1]]))
assert_equal(transformacion_PUMA(0, pi/2, 0), Matrix([[0,-1,0,0], [0,0,-1,0], [1,0,0,l1+l2+l3], [0,0,0,1]]))
assert_equal(transformacion_PUMA(0, 0, pi/2), Matrix([[0,-1,0,l2], [0,0,-1,0], [1,0,0,l1+l3], [0,0,0,1]]))
Explanation: Cree una función que dados los angulos del manipulador devuelva la transformación total del manipulador (ayudese de la función creada en el segundo problema).
End of explanation
def DH_numerico(a, d, α, θ):
    # YOUR CODE HERE
    raise NotImplementedError()
    
def cinematica_PUMA(q1, q2, q3):
    # Considere que las longitudes son todas iguales a 1
    l1, l2, l3 = 1, 1, 1
    from numpy import pi
    # YOUR CODE HERE
    raise NotImplementedError()
    return A1, A2, A3
def grafica_PUMA(q1, q2, q3):
    from numpy import matrix
    # YOUR CODE HERE
    raise NotImplementedError()
    fig = figure(figsize=(8, 8))
    ax = fig.add_subplot(111, projection='3d')
    ax.plot(xs, ys, zs, "-o")
    
    ax.set_xlim(-1.1, 1.1)
    ax.set_ylim(-1.1, 1.1)
    ax.set_zlim(-0.1, 2.1)
    return ax
%matplotlib inline
from matplotlib.pyplot import figure, plot, style
from mpl_toolkits.mplot3d import Axes3D
style.use("ggplot")
from numpy.testing import assert_allclose
from numpy import array
ax = grafica_PUMA(0,0.5,0.5)
ls = ax.get_lines()
assert_allclose(ls[0].get_xdata(), array([0, 0, 0.8775, 1.417885]), rtol=1e-01, atol=1e-01)
assert_allclose(ls[0].get_ydata(), array([-0.0384900179, 0, 0.00915,  0.03809]), rtol=1e-01, atol=1e-01)
Explanation: Cree una función que dados los angulos del manipulador, grafique las posiciones de los eslabones del manipulador del primer punto (ayudese de las funciones creadas en el primer y segundo problemas, modificadas ligeramente para aceptar matrices numéricas, así como la función creada en la práctica anterior para la graficación de un sistema robótico).
End of explanation
%matplotlib inline
from matplotlib.pyplot import figure, plot, style
from mpl_toolkits.mplot3d import Axes3D
style.use("ggplot")
from ipywidgets import interact
from numpy import pi
τ = 2*pi
# YOUR CODE HERE
raise NotImplementedError()
from nose.tools import assert_almost_equal
from numpy import pi
τ = 2*pi
Explanation: Utilice la función interact para manipular la posición del manipulador, de tal manera que su posición sea aproximadamente $q_1=0.6rad$, $q_2=0.2rad$ y $q_3 = -0.8rad$
End of explanation
<END_TASK>