% 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).