Python, 24 pasos (trabajo en progreso)
La idea era resolver primero el problema continuo, reduciendo enormemente el espacio de búsqueda y luego cuantizando el resultado en la cuadrícula (simplemente redondeando al punto de cuadrícula más cercano y buscando los 8 cuadrados circundantes)
Configuré la ruta como una suma de funciones trigonométricas (a diferencia de los polinomios, no divergen y son más fáciles de controlar). También controlo la velocidad directamente en lugar de la aceleración, porque es más fácil imponer la condición límite simplemente multiplicando una función de ponderación que tiende a 0 al final.
Mi función objetivo consiste en
un puntaje exponencial para la aceleración> 10
puntaje polinómico para la distancia euclidiana entre el último punto y el objetivo,
puntaje constante alto para cada intersección con un muro, disminuyendo hacia los bordes del muro
Para minimizar el puntaje, lo tiro todo a la optimización de Nelder-Mead y espero unos segundos. El algoritmo siempre logra llegar al final, deteniéndose allí y sin exceder la aceleración máxima, pero tiene problemas con las paredes. El camino se teletransporta a través de las esquinas y se queda atascado allí, o se detiene junto a una pared con la portería al otro lado (imagen izquierda)
Durante las pruebas, tuve suerte y encontré un camino que se hizo con garabatos de una manera prometedora (imagen derecha) y después de ajustar los parámetros un poco más, podría usarlo como una suposición inicial para una optimización exitosa.
Cuantización
Después de encontrar una ruta paramétrica, llegó el momento de eliminar los puntos decimales. Mirar el vecindario 3x3 reduce el espacio de búsqueda de aproximadamente 300 ^ N a 9 ^ N, pero aún es demasiado grande y aburrido para implementar. Antes de seguir este camino, intenté agregar un término "Ajustar a la cuadrícula" a la función objetivo (las partes comentadas). Cien pasos más de optimización con el objetivo actualizado y simplemente redondear fueron suficientes para obtener la solución.
[(9, -1), (4, 0), (1, 1), (2, 2), (-1, 2), (-3, 4), (-3, 3), (-2 , 3), (-2, 2), (-1, 1), (0, 0), (1, -2), (2, -3), (2, -2), (3, -5 ), (2, -4), (1, -5), (-2, -3), (-2, -4), (-3, -9), (-4, -4), (- 5, 8), (-4, 8), (5, 8)]
El número de pasos fue fijo y no forma parte de la optimización, pero dado que tenemos una descripción analítica de la ruta (y dado que la aceleración máxima está muy por debajo de 10), podemos reutilizarla como punto de partida para una mayor optimización con un número menor de pasos de tiempo
from numpy import *
from scipy.optimize import fmin
import matplotlib.pyplot as plt
from matplotlib.collections import LineCollection as LC
walls = array([[[0,0],[500,0]], # [[x0,y0],[x1,y1]]
[[500,0],[500,400]],
[[500,400],[0,400]],
[[0,400],[0,0]],
[[200,200],[100,200]],
[[100,200],[100,100]],
[[100,100],[200,100]],
[[250,300],[250,200]],
[[300,300],[300,100]],
[[300,200],[400,200]],
[[300,100],[400,100]],
[[100,180],[120, 200]], #debug walls
[[100,120],[120, 100]],
[[300,220],[320, 200]],
#[[320,100],[300, 120]],
])
start = array([120,180])
goal = array([320,220])
###################################
# Boring stuff below, scroll down #
###################################
def weightedintersection2D(L1, L2):
# http://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect
p = L1[0]
q = L2[0]
r = L1[1]-L1[0]
s = L2[1]-L2[0]
d = cross(r,s)
if d==0: # parallel
if cross(q-p,r)==0: return 1 # overlap
else:
t = cross(q-p,s)*1.0/d
u = cross(q-p,r)*1.0/d
if 0<=t<=1 and 0<=u<=1: return 1-0*abs(t-.5)-1*abs(u-.5) # intersect at p+tr=q+us
return 0
def sinsum(coeff, tt):
'''input: list of length 2(2k+1),
first half for X-movement, second for Y-movement.
Of each, the first k elements are sin-coefficients
the next k+1 elements are cos-coefficients'''
N = len(coeff)/2
XS = [0]+list(coeff[:N][:N/2])
XC = coeff[:N][N/2:]
YS = [0]+list(coeff[N:][:N/2])
YC = coeff[N:][N/2:]
VX = sum([XS[i]*sin(tt*ww[i]) + XC[i]*cos(tt*ww[i]) for i in range(N/2+1)], 0)
VY = sum([YS[i]*sin(tt*ww[i]) + YC[i]*cos(tt*ww[i]) for i in range(N/2+1)], 0)
return VX*weightfunc, VY*weightfunc
def makepath(vx, vy):
# turn coordinates into line segments, to check for intersections
xx = cumsum(vx)+start[0]
yy = cumsum(vy)+start[1]
path = []
for i in range(1,len(xx)):
path.append([[xx[i-1], yy[i-1]],[xx[i], yy[i]]])
return path
def checkpath(path):
intersections = 0
for line1 in path[:-1]: # last two elements are equal, and thus wrongly intersect each wall
for line2 in walls:
intersections += weightedintersection2D(array(line1), array(line2))
return intersections
def eval_score(coeff):
# tweak everything for better convergence
vx, vy = sinsum(coeff, tt)
path = makepath(vx, vy)
score_int = checkpath(path)
dist = hypot(*(path[-1][1]-goal))
score_pos = abs(dist)**3
acc = hypot(diff(vx), diff(vy))
score_acc = sum(exp(clip(3*(acc-10), -10,20)))
#score_snap = sum(abs(diff(vx)-diff(vx).round())) + sum(abs(diff(vy)-diff(vy).round()))
print score_int, score_pos, score_acc#, score_snap
return score_int*100 + score_pos*.5 + score_acc #+ score_snap
######################################
# Boring stuff above, scroll to here #
######################################
Nw = 4 # <3: paths not squiggly enough, >6: too many dimensions, slow
ww = [1*pi*k for k in range(Nw)]
Nt = 30 # find a solution with tis many steps
tt = linspace(0,1,Nt)
weightfunc = tanh(tt*30)*tanh(30*(1-tt)) # makes sure end velocity is 0
guess = random.random(4*Nw-2)*10-5
guess = array([ 5.72255365, -0.02720178, 8.09631272, 1.88852287, -2.28175362,
2.915817 , 8.29529905, 8.46535503, 5.32069444, -1.7422171 ,
-3.87486437, 1.35836498, -1.28681144, 2.20784655]) # this is a good start...
array([ 10.50877078, -0.1177561 , 4.63897574, -0.79066986,
3.08680958, -0.66848585, 4.34140494, 6.80129358,
5.13853914, -7.02747384, -1.80208349, 1.91870184,
-4.21784737, 0.17727804]) # ...and it returns this solution
optimsettings = dict(
xtol = 1e-6,
ftol = 1e-6,
disp = 1,
maxiter = 1000, # better restart if not even close after 300
full_output = 1,
retall = 1)
plt.ion()
plt.axes().add_collection(LC(walls))
plt.xlim(-10,510)
plt.ylim(-10,410)
path = makepath(*sinsum(guess, tt))
plt.axes().add_collection(LC(path, color='red'))
plt.plot(*start, marker='o')
plt.plot(*goal, marker='o')
plt.show()
optres = fmin(eval_score, guess, **optimsettings)
optcoeff = optres[0]
#for c in optres[-1][::optimsettings['maxiter']/10]:
for c in array(optres[-1])[logspace(1,log10(optimsettings['maxiter']-1), 10).astype(int)]:
vx, vy = sinsum(c, tt)
path = makepath(vx,vy)
plt.axes().add_collection(LC(path, color='green'))
plt.show()
Para hacer: GUI que le permite dibujar una ruta inicial para obtener un sentido aproximado de la dirección. Cualquier cosa es mejor que el muestreo aleatorio desde un espacio de 14 dimensiones.
