Implemente el código para las funciones que devuelvan matrices de rotación y traslación necesarias para implementar la cinemática directa de un manipulador Stanford.
In [ ]:
def rotacion_x(θ):
# YOUR CODE HERE
raise NotImplementedError()
def rotacion_y(θ):
# YOUR CODE HERE
raise NotImplementedError()
def rotacion_z(θ):
# YOUR CODE HERE
raise NotImplementedError()
def traslacion_x(x):
# YOUR CODE HERE
raise NotImplementedError()
def traslacion_y(y):
# YOUR CODE HERE
raise NotImplementedError()
def traslacion_z(z):
# YOUR CODE HERE
raise NotImplementedError()
In [ ]:
from numpy.testing import assert_allclose
from numpy import eye, pi, matrix, array
assert_allclose(rotacion_x(0), eye(4))
assert_allclose(rotacion_x(pi), matrix([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]), rtol=1e-05, atol=1e-05)
assert_allclose(rotacion_y(0), eye(4))
assert_allclose(rotacion_y(pi), matrix([[-1, 0, 0, 0], [0, 1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]), rtol=1e-05, atol=1e-05)
assert_allclose(rotacion_z(0), eye(4))
assert_allclose(rotacion_z(pi), matrix([[-1, 0, 0, 0], [0, -1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]), rtol=1e-05, atol=1e-05)
assert_allclose(traslacion_x(0), eye(4), rtol=1e-05, atol=1e-05)
assert_allclose(traslacion_x(1), matrix([[1, 0, 0, 1], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]), rtol=1e-05, atol=1e-05)
assert_allclose(traslacion_y(0), eye(4), rtol=1e-05, atol=1e-05)
assert_allclose(traslacion_y(1), matrix([[1, 0, 0, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]]), rtol=1e-05, atol=1e-05)
assert_allclose(traslacion_z(0), eye(4), rtol=1e-05, atol=1e-05)
assert_allclose(traslacion_z(1), matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 1], [0, 0, 0, 1]]), rtol=1e-05, atol=1e-05)
Implemente una función que devuelva las matrices de transformación homogeneas, que describen la cinemática directa de un manipulador Stanford.
In [ ]:
def cinematica_stanford(q1, q2, q3):
# Tomamos en cuenta que la longitudes son 1
l1, l2 = 1, 1
# YOUR CODE HERE
raise NotImplementedError()
return A1, A2, A3
In [ ]:
cinematica_stanford(0,0,0)
In [ ]:
from numpy.testing import assert_allclose
from numpy import eye, pi, matrix, array
A1, A2, A3 = cinematica_stanford(0, 0, 0)
assert_allclose(A1, matrix([[1,0,0,0], [0,1,0,0], [0,0,1,1], [0,0,0,1]]), rtol=1e-05, atol=1e-5)
assert_allclose(A2, eye(4), rtol=1e-05, atol=1e-5)
assert_allclose(A3, matrix([[1,0,0,0], [0,1,0,1], [0,0,1,0], [0,0,0,1]]), rtol=1e-05, atol=1e-5)
Cree una función que tome como argumentos los valores de posición (angular o lineal) de los grados de libertad de un manipulador Stanford, calcule la cinemática directa y grafique en tres dimensiones un esquema de los eslabones del manipulador.
In [ ]:
def grafica_stanford(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
ax = grafica_stanford(0,0.5,0.5)
ls = ax.get_lines()
assert_allclose(ls[0].get_xdata(), array([0, 0, 0, 0.0290876490]), rtol=1e-01, atol=1e-01)
assert_allclose(ls[0].get_ydata(), array([-0.0384900179, 0, 0, 1.316]), rtol=1e-01, atol=1e-01)
Utilice la función interact para controlar el movimiento de un manipulador Stanford y posicione al manipulador aproximadamente en $q_1 = 30^o$, $q_2 = 20^o$ y $q_3 = 0.2m$
In [ ]:
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