Defina una función que obtenga la cinemática inversa de un pendulo doble.
In [ ]:
def ci_pendulo_doble(x, y):
# tome en cuenta que las longitudes de los eslabones son 2 y 2
l1, l2 = 2, 2
from numpy import arccos, arctan2, sqrt
# YOUR CODE HERE
raise NotImplementedError()
return q1, q2
In [ ]:
from numpy.testing import assert_allclose
assert_allclose(ci_pendulo_doble(4, 0), (0,0))
assert_allclose(ci_pendulo_doble(0, 4), (1.57079632,0))
Obtenga las posiciones en el espacio articular, $q_1$ y $q_2$, necesarias para que el punto final del pendulo doble llegue a las coordenadas $p_1 = (0,1)$, $p_2 = (1,3)$ y $p_3 = (3,2)$.
In [ ]:
# YOUR CODE HERE
raise NotImplementedError()
In [ ]:
from numpy.testing import assert_allclose
assert_allclose((q11, q21),(0.25268 , 2.636232), rtol=1e-05, atol=1e-05)
In [ ]:
from numpy.testing import assert_allclose
assert_allclose((q12, q22),(0.589988, 1.318116), rtol=1e-05, atol=1e-05)
In [ ]:
from numpy.testing import assert_allclose
assert_allclose((q13, q23),(0.14017 , 0.895665), rtol=1e-05, atol=1e-05)
Genere las trayectorias necesarias para que el pendulo doble se mueva del punto $p_1$ al punto $p_2$ en $2s$, del punto $p_2$ al punto $p_3$ en $2s$ y del punto $p_3$ al punto $p_1$ en $2s$.
Utiliza 100 puntos por segundo y asegurate de guardar las trayectorias generadas en las variables correctas para que
q1s
yq2s
tengan las trayectorias completas.
In [ ]:
from generacion_trayectorias import grafica_trayectoria
# YOUR CODE HERE
raise NotImplementedError()
q1s = q1s1 + q1s2 + q1s3
q2s = q2s1 + q2s2 + q2s3
In [ ]:
from numpy.testing import assert_allclose
assert_allclose((q1s[0], q1s[-1]),(0.25268, 0.25268), rtol=1e-05, atol=1e-05)
In [ ]:
from numpy.testing import assert_allclose
assert_allclose((q2s[0], q2s[-1]),(2.636232, 2.636232), rtol=1e-05, atol=1e-05)
Cree una animación con las trayectorias generadas y las funciones proporcionadas a continuación (algunas funciones estan marcadas con comentarios en donde hace falta agregar código).
In [ ]:
from matplotlib.pyplot import figure, style
from matplotlib import animation, rc
rc('animation', html='html5')
from numpy import sin, cos, arange
fig = figure(figsize=(8, 8))
axi = fig.add_subplot(111, autoscale_on=False, xlim=(-0.6, 3.1), ylim=(-0.6, 3.1))
linea, = axi.plot([], [], "-o", lw=2, color='gray')
def cd_pendulo_doble(q1, q2):
l1, l2 = 2, 2
# YOUR CODE HERE
raise NotImplementedError()
return xs, ys
def inicializacion():
'''Esta funcion se ejecuta una sola vez y sirve para inicializar el sistema'''
linea.set_data([], [])
return linea
def animacion(i):
'''Esta funcion se ejecuta para cada cuadro del GIF'''
# YOUR CODE HERE
raise NotImplementedError()
linea.set_data(xs, ys)
return linea
ani = animation.FuncAnimation(fig, animacion, arange(1, len(q1s)), interval=10, init_func=inicializacion)
ani
In [ ]:
from numpy.testing import assert_allclose
assert_allclose(cd_pendulo_doble(0, 0), ([0,2,4], [0,0,0]), rtol=1e-05, atol=1e-05)
assert_allclose(cd_pendulo_doble(1.57079632,0), ([0, 0, 0],[0, 2, 4]), rtol=1e-05, atol=1e-05)