El objetivo de esta práctica es generar una trayectoria para un robot manipulador, de tal manera que no tenga cambios bruscos de posición o velocidad.
El algoritmo general que vamos a usar se llama LSPB (Linear segment with parabolic blend), en el cual vamos a tener una velocidad de crucero para el manipulador, así como un periodo en el que el manipulador acelerará constantemente y otro periodo en el que desacelererá.
Una trayectoria generada por este método se ve asi:
En la primer sección se tiene una aceleración constante, en la segunda una velocidad constante y en la tercera una aceleración constante de signo contrario a la primera.
El método por el que se generó no es particularmente dificil, tan solo es un poco engorroso de programar, por lo que para facilidad de esta práctica, este método ya esta programado, tan solo hay que importar el código:
In [ ]:
from generacion_trayectorias import grafica_trayectoria
%matplotlib inline
Vamos a hacer una prueba en primer lugar:
In [ ]:
ts, qs, q̇s, q̈s = grafica_trayectoria(0, 2, 0, 1, 1000)
En la gráfica anterior podemos ver no solo la posición en el primer cuadro, si no tambien la velocidad y la aceleración en el segundo y tercero, de tal manera que nos damos una mejor idea de la trayectoria.
Vamos a generar un conjunto de trayectorias para un ejemplo, primero empecemos importando pi:
In [ ]:
from numpy import pi
τ = 2*pi
Vamos a generar una trayectoria en la que en los primero dos segundos se mueva de $0^o$ a $90^o$, en los segundos dos segundos de $90^o$ a $-60^o$ y en los ultimos seis segundos de $-60^o$ a $240^o$:
In [ ]:
ts, q1, q̇1, q̈1 = grafica_trayectoria(0, 2, 0, τ/4, 100)
In [ ]:
ts, q2, q̇2, q̈2 = grafica_trayectoria(2, 4, τ/4, -τ/6, 100)
In [ ]:
ts, q3, q̇3, q̈3 = grafica_trayectoria(4, 10, -τ/6, 2*τ/3, 300)
Si quiero concatenar todos estos arreglos que generé, tan solo tengo que sumarlos:
In [ ]:
qs = q1 + q2 + q3
Esta trayectoria la podemos graficar:
In [ ]:
from matplotlib.pyplot import figure, style
In [ ]:
from numpy import linspace
fig = figure(figsize=(17, 5))
ax = fig.gca()
ts = linspace(0, 10, 500)
ax.plot(ts, qs)
Pero mas importante, puedo generar una animación, tomando en cuenta que es un pendulo simple:
In [ ]:
# Se importa la funcion animation para crear la animacion, y rc para poder mostrar el video
# directamente en el notebook
from matplotlib import animation, rc
rc('animation', html='html5')
# Se importan las funciones necesarias para calcular la cinematica directa e inversa
from numpy import sin, cos, arange
In [ ]:
# Se define una funcion para calcular la cinematica directa del sistema
def cinematica_directa_pendulo(q1):
# Se definen constantes utilizadas para graficar el sistema
l1, l2 = 1, 1
xs = [0, l1*cos(q1)]
ys = [0, l1*sin(q1)]
return xs, ys
In [ ]:
# Se define el tamaño de la figura
fig = figure(figsize=(8, 8))
# Se define una sola grafica en la figura y se dan los limites de los ejes x y y
axi = fig.add_subplot(111, autoscale_on=False, xlim=(-1.1, 1.1), ylim=(-1.1, 1.1))
# Se utilizan graficas de linea para el eslabon del pendulo
linea, = axi.plot([], [], "-o", lw=2, color='gray')
def inicializacion():
'''Esta funcion se ejecuta una sola vez y sirve para inicializar el sistema'''
# Se inicializa la linea vacia para evitar que al principio exista una linea en la grafica
linea.set_data([], [])
return linea
def animacion(i):
'''Esta funcion se ejecuta para cada cuadro del GIF'''
# Se obtienen las coordenadas x y y para el eslabon
xs, ys = cinematica_directa_pendulo(qs[i])
# Se actualiza el estado de la linea con las coordenadas calculadas
linea.set_data(xs, ys)
return linea
# Se hace la animacion dandole la funcion que se debe ejecutar para cada cuadro, el numero de cuadros
# que se debe de hacer, el periodo de cada cuadro y la funcion inicial
ani = animation.FuncAnimation(fig, animacion, arange(1, len(qs)), interval=20, init_func=inicializacion)
ani