% multi-agent path finding for grid maps and the sum-of-costs objective
%
% The solution is described in
%
% "Modeling and Solving the Multi-Agent Pathfinding Problem in Picat"
% R. Bartak, N.F. Zhou, R. Stern, E. Boyarski, and P. Surynek
% ICTAI'17
import sat.
main([InsFile]) =>
printf("solving %s\n",InsFile),
cl(InsFile),
main.
main =>
garbage_collect(800000000),
ins(M,As),
gen_rel(M),
N = max([max(Row) : Row in M]), % N is the number of nodes in the graph
path(N,to_array(As)).
test =>
M = {
{0,1,2,3,4,5,6,0},
{7,8,9,10,11,12,13,14},
{15,16,17,18,19,20,21,22},
{23,24,25,26,27,28,29,30},
{31,32,33,34,0,35,36,37},
{38,39,40,41,42,43,44,0},
{45,0,46,47,48,49,50,51},
{52,53,54,55,56,57,58,0}},
As = [(45,23),(32,55),(37,38),(21,11),(24,32)],
gen_rel(M),
N = max([max(Row) : Row in M]), % N is the number of nodes in the graph
path(N,to_array(As)).
% convert the matrix to an adjacency relation named neibs/2
gen_rel(M) =>
RowColRel = [],
Rel = [],
foreach (R in 1..len(M), C in 1..len(M[1]), M[R,C] !== 0)
Neibs = [M[R1,C1] : (R1,C1) in [(R,C),(R,C+1),(R,C-1),(R-1,C),(R+1,C)],
R1 >=1, R1 =< len(M), C1 >= 1, C1 =< len(M[1]),
M[R1,C1] !== 0],
Rel := [$neibs(M[R,C],Neibs)|Rel],
RowColRel := [$row_col(M[R,C],R,C)|RowColRel]
end,
cl_facts(Rel,[$neibs(+,-)]),
cl_facts(RowColRel,[$row_col(+,-,-)]).
% N : the number of nodes in the graph
% As : list of agents [(V1,FV1),(V2,FV2),...,(Vk,FVk)],
% where Vi is the initial location and FVi is the final location of agent i
% For each agent and each time point between 1..PathLen+1, create a frame.
path(N,As) =>
K = len(As),
lower_upper_bounds(to_list(As),LB,UB),
between(LB,UB,M),
M1 = M+1,
printf("trying %w\n",M),
B = new_array(M1,K,N),
% Initialize the first and last states
foreach(A in 1..K)
(V,FV) = As[A],
B[1,A,V] = 1,
preprocess_forward(A,V,M1,N,B),
B[M1,A,FV] = 1,
preprocess_backward(A,FV,M1,N,B)
end,
B :: 0..1,
% Each agent occupies exactly one vertex at each time.
foreach (T in 1..M1, A in 1..K)
sum([B[T,A,V] : V in 1..N]) #= 1
end,
% No two agents occupy the same vertex at any time.
foreach(T in 1..M1, V in 1..N)
sum([B[T,A,V] : A in 1..K]) #=< 1
end,
% Every transition is valid
foreach(T in 1..M, A in 1..K, V in 1..N)
neibs(V,Neibs),
B[T,A,V] #=> sum([B[T+1,A,U] : U in Neibs]) #>= 1
end,
%
end_time(B,As,K,M1,ET),
%
writeln(solving),
% solve([dump],B),
solve($[min(sum(ET))],B), % minimizing sum-of-costs
% writeln(B),
writeln(len=M),
writeln(soc=sum(ET)).
% the end time of agent A is ET[A]
end_time(B,As,K,MaxT,ET) =>
ET = new_array(K),
ET :: 1..MaxT,
foreach (A in 1..K, T in 1..MaxT)
(_V,FV) = As[A],
EndAtT #<=> (ET[A] #= T),
if T > 1 then
EndAtT #=> #~ B[T-1,A,FV]
end,
foreach (T1 in T..MaxT)
EndAtT #=> B[T1,A,FV]
end
end.
% foreach vertex U, if U is at least distance D away from V,
% then agent A cannot occupy vertex U at time T, T+1, ..., T+D-1
preprocess_forward(A,V,MaxT,N,B) =>
foreach (U in 1..N, V !== U)
if manhattan_dist((V,U),Dist) then
foreach (T1 in 1..min(Dist,MaxT))
B[T1,A,U] = 0
end
end
end.
% foreach vertex U, if U is at least distance D away from FV,
% then agent A cannot occupy vertex U at time MaxT, MaxT-1, ..., MaxT-D+1
preprocess_backward(A,FV,MaxT,N,B) =>
foreach (U in 1..N, FV !== U)
if manhattan_dist((FV,U),Dist) then
foreach (T1 in MaxT..-1..max(1,MaxT-Dist+1))
B[T1,A,U] = 0
end
end
end.
lower_upper_bounds(As,LB,UB) =>
lower_upper_bounds(As,0,LB,0,UB).
lower_upper_bounds([],LB0,LB,UB0,UB) => LB = LB0, UB = UB0.
lower_upper_bounds([A|As],LB0,LB,UB0,UB) =>
shortest_path_cost(A,Cost),
lower_upper_bounds(As,max(LB0,Cost),LB,UB0+Cost,UB).
table (+,min)
shortest_path_cost((V,V),Cost) => Cost = 0.
shortest_path_cost((V,FV),Cost) =>
neibs(V,Neibs),
member(NextV,Neibs),
shortest_path_cost((NextV,FV),Cost1),
Cost = Cost1+1.
% Manhattan distance
manhattan_dist((U,V),Cost) =>
row_col(U,URow,UCol),
row_col(V,VRow,VCol),
Cost = abs(URow-VRow)+abs(UCol-VCol).