% multi-agent path finding for grid maps and the makespan 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 => test_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)). % convert the matrix to an adjacency relation named neibs/2 gen_rel(M) => RowColRel = [], Rel = [], NRows = len(M), NCols = len(M[1]), 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 =< NRows, C1 >= 1, C1 =< NCols, 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, writeln(solving), % % solve([dump],B), solve([threads],B), % output_plan(M,K,N,B), writeln(len=M). % 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_dist(A,Cost), lower_upper_bounds(As,max(LB0,Cost),LB,UB0+Cost,UB). table (+,min) shortest_dist((V,V),Cost) => Cost = 0. shortest_dist((V,FV),Cost) => neibs(V,Neibs), member(NextV,Neibs), shortest_dist((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). output_plan(M,K,N,B) => foreach (A in 1..K) printf("Agent-%w:\n", A), foreach (T in 1..M+1, V in 1..N) if B[T,A,V] == 1 then row_col(V,Row,Col), printf("(%w,%w)",Row,Col), if T <= M then print("->") else println(".") end end end, nl end. ins(M, As) => M = { {1,2,3,4,5,6,7,8,9,10,11,12,13,14,0,0,0,15,16,17,18,19,0,20,21,22,0,23,24,0,0,0}, {25,0,0,26,27,0,28,29,30,31,32,33,0,34,35,36,37,38,39,40,41,42,43,44,45,46,47,48,49,50,51,52}, {53,54,55,0,56,0,57,0,58,59,0,60,0,0,61,62,63,64,65,66,67,68,69,70,71,72,73,74,75,0,76,77}, {78,79,80,81,82,83,84,85,86,87,88,89,0,90,0,91,92,0,93,94,95,96,0,97,98,99,100,101,102,103,104,105}, {0,106,107,108,0,109,110,111,112,113,114,115,116,117,118,119,120,0,121,122,123,124,125,126,0,127,128,129,130,131,132,133}, {0,0,134,135,136,137,138,139,140,141,142,143,144,145,146,147,148,149,150,151,0,152,153,154,155,156,157,0,0,158,159,0}, {160,161,162,0,163,0,164,165,166,167,168,0,169,0,170,171,172,173,0,174,0,0,175,0,176,177,178,179,180,0,181,0}, {182,183,184,185,186,187,188,189,190,191,0,192,193,194,195,0,196,197,0,198,0,199,200,0,201,202,203,204,205,206,207,208}, {0,209,210,211,212,213,214,215,216,217,218,219,220,221,222,223,0,224,225,0,0,226,227,228,0,229,230,0,231,0,232,233}, {234,235,236,237,238,239,0,240,241,0,242,243,244,245,246,247,248,0,249,250,251,252,253,254,255,0,256,257,258,0,259,260}, {261,0,0,262,263,264,265,266,0,267,268,269,270,0,271,272,273,0,274,0,275,276,277,278,0,279,280,281,282,283,284,285}, {286,287,288,289,290,291,0,292,293,294,295,0,296,297,0,298,299,0,0,300,301,0,302,303,304,305,306,307,308,309,310,311}, {0,312,313,0,314,315,316,317,318,0,319,320,0,0,321,322,0,323,324,325,0,326,327,328,329,330,331,332,333,334,335,336}, {337,0,338,339,0,0,340,341,342,343,344,345,346,347,348,349,350,351,352,353,354,355,0,0,356,0,357,358,0,359,0,0}, {360,361,362,0,363,364,365,366,367,368,369,0,370,371,0,372,373,374,0,375,376,377,378,379,380,381,382,383,384,385,386,387}, {0,388,389,0,0,0,0,0,0,0,390,391,392,393,394,395,396,397,398,399,400,401,402,403,0,404,405,406,407,408,0,409}, {410,411,412,413,414,415,0,416,417,0,418,419,0,420,421,422,423,424,0,425,426,427,428,429,430,431,432,433,434,435,436,437}, {438,0,439,440,441,0,0,442,443,444,445,0,446,447,448,449,0,450,451,452,453,454,455,456,457,458,459,460,461,462,463,464}, {0,465,466,467,468,469,470,471,472,473,474,475,476,477,478,0,479,480,481,482,483,484,485,486,0,0,487,488,0,489,490,0}, {491,492,493,494,495,496,497,498,499,0,500,0,501,502,503,504,505,506,507,508,509,0,510,511,512,513,514,0,0,515,516,517}, {518,519,0,520,521,0,522,523,524,525,526,527,528,529,530,531,532,533,0,534,535,536,537,538,539,540,0,541,542,543,544,545}, {546,0,547,548,549,550,551,552,0,553,554,555,0,556,557,558,559,560,561,0,0,562,563,0,0,564,0,0,565,566,0,567}, {568,569,570,571,572,573,574,0,575,576,577,578,579,580,581,0,582,583,584,585,586,0,0,587,588,0,589,590,591,592,0,0}, {593,594,595,596,597,598,599,0,0,600,0,601,602,603,604,605,606,607,0,608,609,610,611,612,0,613,0,614,615,0,0,616}, {0,0,617,618,0,0,619,0,620,621,622,623,0,0,0,624,625,0,626,627,0,628,629,0,630,631,0,632,633,634,635,636}, {637,638,639,640,641,0,642,643,644,0,645,0,646,647,648,649,650,651,652,653,654,655,656,657,0,658,659,660,661,662,663,0}, {664,665,0,0,666,667,668,669,670,0,671,672,673,0,674,675,676,677,678,679,680,681,682,683,684,0,0,685,686,687,0,0}, {0,688,689,690,691,692,693,694,0,0,695,696,697,698,699,700,701,702,703,704,705,706,707,708,0,709,0,710,711,712,713,714}, {715,716,717,718,719,720,0,0,721,722,723,724,725,726,727,0,728,729,730,731,732,733,0,734,735,0,736,737,738,0,739,740}, {741,0,742,743,744,745,746,747,748,749,750,751,752,753,754,755,756,757,758,759,760,0,761,762,0,0,763,764,765,766,0,767}, {768,769,770,771,772,773,774,0,775,0,776,777,778,0,779,0,0,780,0,781,782,783,784,0,785,786,787,788,789,790,791,792}, {793,794,795,796,797,798,799,800,801,0,0,802,803,804,805,806,0,807,808,809,810,811,812,813,814,815,816,817,818,0,819,820}}, As = [(782,674),(293,711),(716,117),(482,287),(407,96),(181,138),(292,206),(380,587),(298,510),(565,360),(406,6),(190,152),(441,79),(182,795),(538,123),(680,196),(89,560),(469,44),(111,350),(783,340),(18,50),(370,588),(159,598),(539,585),(209,133),(702,134),(49,755),(706,576),(471,102),(355,785)].