Defina una función que tome como argumentos los parametros de Denavit - Hartenberg, y cree una matriz de transformación homogénea.
In [ ]:
def DH_simbolico(a, d, α, θ):
from sympy import Matrix, sin, cos
# YOUR CODE HERE
raise NotImplementedError()
In [ ]:
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.
In [ ]:
def cinematica_PUMA(q1, q2, q3):
from sympy import pi, var
var("l1:4")
# YOUR CODE HERE
raise NotImplementedError()
return A1, A2, A3
In [ ]:
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).
In [ ]:
def transformacion_PUMA(q1, q2, q3):
from sympy import pi, var
var("l1:4")
# YOUR CODE HERE
raise NotImplementedError()
In [ ]:
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).
In [ ]:
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
In [ ]:
%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$
In [ ]:
%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()
In [ ]:
from nose.tools import assert_almost_equal
from numpy import pi
τ = 2*pi