# Problemas

---
Defina una función que tome como argumentos los parametros de Denavit - Hartenberg, y cree una matriz de transformación homogénea.


```python
def DH_simbolico(a, d, α, θ):
    from sympy import Matrix, sin, cos
    # YOUR CODE HERE
    raise NotImplementedError()
```


```python
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]]))
```

---
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.


```python
def cinematica_PUMA(q1, q2, q3):
    from sympy import pi, var
    var("l1:4")
    # YOUR CODE HERE
    raise NotImplementedError()
    return A1, A2, A3
```


```python
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]]))
```

---
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).


```python
def transformacion_PUMA(q1, q2, q3):
    from sympy import pi, var
    var("l1:4")
    # YOUR CODE HERE
    raise NotImplementedError()
```


```python
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]]))
```

---
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).


```python
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
```


```python
%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)
```

---
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$


```python
%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()
```


```python
from nose.tools import assert_almost_equal
from numpy import pi
τ = 2*pi
```

3ac85c746d7d80e80fa64830f08f904cd3172a80
ipynb
Jupyter Notebook
Practicas/practica4/Problemas.ipynb
robblack007/clase-cinematica-robot
d542839a9ad31bd01a9cfbd8fcdbfd1933a20bde
MIT
Practicas/practica4/Problemas.ipynb
robblack007/clase-cinematica-robot
d542839a9ad31bd01a9cfbd8fcdbfd1933a20bde
MIT
2016-01-27T05:42:40.000Z
2016-02-02T17:36:55.000Z
Practicas/practica4/Problemas.ipynb
robblack007/clase-cinematica-robot
d542839a9ad31bd01a9cfbd8fcdbfd1933a20bde
MIT
Qwen/Qwen-72B
1. YES
2. YES