1%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    2%
    3% FILE: Delivery-BAT/delivery.pl
    4% Time-stamp: <03/12/26 20:33:09 ssardina>
    5%
    6%       BAT axiomatization of the delivery robot
    7%
    8%    LAST REVISED:                    (Sebastian Sardina) 
    9%    TESTED: SWI Prolog 5.2.8 under RedHat Linux 6.2-9.0
   10%            ECLIPSE 5.7 under RedHat Linux 6.2-9.0
   11%    DESCRIPTION: This is a controller for a delivery robot
   12%
   13%           For more information on Golog and some of its variants, see:
   14%               http://www.cs.toronto.edu/~cogrobo/
   15%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   16%
   17%                             May 15, 2001
   18%
   19% This software was developed by the Cognitive Robotics Group under the
   20% direction of Hector Levesque and Ray Reiter.
   21% 
   22%        Do not distribute without permission.
   23%        Include this notice in any copy made.
   24% 
   25% 
   26%         Copyright (c) 2000-2002 by The University of Toronto,
   27%                        Toronto, Ontario, Canada.
   28% 
   29%                          All Rights Reserved
   30% 
   31% Permission to use, copy, and modify, this software and its
   32% documentation for non-commercial research purpose is hereby granted
   33% without fee, provided that the above copyright notice appears in all
   34% copies and that both the copyright notice and this permission notice
   35% appear in supporting documentation, and that the name of The University
   36% of Toronto not be used in advertising or publicity pertaining to
   37% distribution of the software without specific, written prior
   38% permission.  The University of Toronto makes no representations about
   39% the suitability of this software for any purpose.  It is provided "as
   40% is" without express or implied warranty.
   41% 
   42% THE UNIVERSITY OF TORONTO DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS
   43% SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND
   44% FITNESS, IN NO EVENT SHALL THE UNIVERSITY OF TORONTO BE LIABLE FOR ANY
   45% SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER
   46% RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF
   47% CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
   48% CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
   49%
   50%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   51% 
   52% This is a Golog delivery robot. It assumes clipping actions to start
   53% and stop motion behaviour, and exogenous requests for deliveries.
   54% There are no sensing actions (other than the exogenous ones).
   55%
   56%  A basic action theory (BAT) is described with:
   57%
   58% -- fun_fluent(fluent)     : for each functional fluent (non-ground)
   59% -- rel_fluent(fluent)     : for each relational fluent (non-ground)
   60%
   61%           e.g., rel_fluent(painted(C)).
   62%           e.g., fun_fluent(color(C)).
   63%
   64% -- prim_action(action)    : for each primitive action (ground)
   65% -- exog_action(action)    : for each exogenous action (ground)
   66%
   67%           e.g., prim_action(clean(C)) :- domain(C,country).
   68%           e.g., exog_action(painte(C,B)):- domain(C,country), domain(B,color).
   69%
   70% -- senses(action,fluent)  : for each sensing action
   71%
   72%           e.g, poss(check_painted(C),  painted(C)).
   73%
   74% -- poss(action,cond)      : when cond, action is executable
   75%
   76%           e.g, poss(clean(C),   and(painted(C),holding(cleanear))).
   77%
   78% -- initially(fluent,value): fluent has value in S0 (ground)
   79%
   80%          e.g., initially(painted(C), false):- domain(C,country), C\=3.
   81%                initially(painted(3), true).
   82%                initially(color(3), blue).
   83%
   84% -- causes_val(action,fluent,value,cond)
   85%          when cond holds, doing act causes functional fluent to have value
   86%
   87%            e.g., causes_val(paint(C2,V), color(C), V, C = C2).
   88%               or causes_val(paint(C,V), color(C), V, true).
   89%
   90% -- causes_true(action,fluent,cond)
   91%          when cond holds, doing act causes relational fluent to hold
   92% -- causes_false(action,fluent,cond)
   93%          when cond holds, doing act causes relational fluent to not hold
   94%
   95%            e.g., causes_true(paint(C2,_), painted(C), C = C2).
   96%               or causes_true(paint(C,_), painted(C), true).
   97%            e.g., causes_false(clean(C2),  painted(C), C = C2).
   98%               or causes_false(clean(C),  painted(C), true).
   99%
  100% -- sort(name,domain_of_sort).      : all sorts used in the domain
  101%
  102%        e.g., varsort(c, colors).
  103%              varsort(temp, temperature).
  104%              color([blue, green, yellow, red]).       
  105%              temperature([-10,0,10,20,30,40]).
  106%
  107%
  108% A high-level program-controller is described with:
  109%
  110% -- proc(name,P): for each procedure P 
  111% -- simulator(N,P): P is the N exogenous action simulator
  112%
  113% The interface for Lego is described with:
  114%
  115% -- actionNum(action, num)  
  116%         action has RCX code num
  117% -- simulateSensing(action)
  118%         sensing result for action should be asked to the user
  119% -- translateSensing(action, sensorValue, sensorResult) 
  120%         translate the sensorValue of action to sensorResult
  121% -- translateExogAction(codeAction, action) 
  122%         translateSensing action name into codeAction and vice-versa
  123%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  124
  125%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  126% A - DOMAINS 
  127%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  128
  129office([central|L]) :- setof(station(X), member(X,[1,6,11,7,9,12,13,16]), L).
  130corridor(L) :- setof(station(X),member(X,[2,3,4,5,8,14,10,15]),L).
  131location(L) :- office(L1), corridor(L2), append(L1,L2,L).
  132
  133direction([north,south,west,east]).
  134degree([90,180,-90,180,-180]).
  135priority([1,2,3,4,5]).
  136
  137% Any place or space between places
  138anyLocation(A)      :- 
  139        location(P),
  140        setof(E, betweenPlaces(E), B), 
  141        append(P,B,A).
  142betweenLocation(P) :-   % A place between two places
  143        location(X1), 
  144        location(X2), 
  145        X1\=X2, 
  146        P = between(X1,X2).
  147
  148%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  149% B - FLUENTS AND SUCCESSOR STATE AXIOMS
  150%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  151
  152% Fluents that should be cached (they are used very often)
  153cache(direction).
  154cache(robotLocation).
  155
  156%cache(_):-fail.
  157
  158%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  159% ROBOT MOVEMENTS
  160%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  161
  162% Current direction of robot: east,south,north or south
  163fun_fluent(direction).           
  164causes_val(turnLeft,  direction, north, direction=east).
  165causes_val(turnLeft,  direction, west,  direction=north).
  166causes_val(turnLeft,  direction, south, direction=west).
  167causes_val(turnLeft,  direction, east,  direction=south).
  168
  169causes_val(turnRight,  direction, north, direction=west).
  170causes_val(turnRight,  direction, west,  direction=south).
  171causes_val(turnRight,  direction, south, direction=east).
  172causes_val(turnRight,  direction, east,  direction=north).
  173
  174causes_val(turnAround,  direction, north,  direction=south).
  175causes_val(turnAround,  direction, west,   direction=east).
  176causes_val(turnAround,  direction, south,  direction=north).
  177causes_val(turnAround,  direction, east,   direction=west).
  178
  179causes_val(turn(D),           direction, V,      rotation(direction,V,D)).
  180causes_val(startLocalization, direction, north,  true).
  181causes_val(setLocation(_,D),  direction, D,      true).
  182
  183% Current location of the robot
  184fun_fluent(robotLocation).          
  185causes_val(goNext,     robotLocation, P, 
  186	               and(getEdge(robotLocation,P2,direction,currentMap),
  187		           P=between(robotLocation,P2)) ).
  188causes_val(reachDest,  robotLocation, P, robotLocation=between(_,P)).
  189causes_val(turnAround, robotLocation, between(P2,P1), robotLocation=between(P1,P2)).
  190causes_val(setLocation(L,_),  robotLocation, L, true).
  191causes_val(startLocalization, robotLocation, station(2), true).
  192
  193% Last location visited by the robot
  194fun_fluent(robotLastPlace).      
  195causes_val(goNext,    robotLastPlace, P, P=robotLocation).
  196
  197% Where is the robot heading when it is moving
  198fun_fluent(robotDestination).    
  199causes_val(goNext,robotDestination,P,getEdge(robotLocation,P,direction,currentMap)).
  200
  201% A segment between two stations is blocked
  202rel_fluent(blocked(_,_)).        
  203causes_true(getStuck,             blocked(P1,P2), robotLocation=between(P1,P2)). 
  204causes_true(getStuck,             blocked(P1,P2), robotLocation=between(P2,P1)). 
  205causes_false(clearRoute(P1,P2),   blocked(P1,P2), true). 
  206causes_false(clearRoute(P1,P2),   blocked(P2,P1), true). 
  207
  208
  209% Should we talk loud?
  210rel_fluent(talking).           
  211causes_true(talk,    talking, true).
  212causes_false(shutup, talking, true).
  213
  214
  215
  216%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  217% ROBOT STATE
  218%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  219
  220% Current state of the robot
  221fun_fluent(robotState).          
  222causes_val(goNext,          robotState, moving,      true). 
  223causes_val(searchPath,      robotState, moving,      true). 
  224causes_val(reachDest,       robotState, reached,     robotState=moving). 
  225causes_val(stop_abnormally, robotState, lost,        true). 
  226causes_val(freezeRobot,     robotState, frozen,      true). 
  227causes_val(resetRobot,      robotState, idle,        true). 
  228causes_val(dropOff,         robotState, waitingPush, true). 
  229causes_val(pushGo,          robotState, readyGo,     robotState=waitingPush). 
  230causes_val(getStuck,        robotState, stuck,       true). 
  231
  232% Current state of the robot
  233rel_fluent(robotLost).          
  234causes_true(stop_abnormally,    robotLost, true).
  235causes_false(setLocation(_,_),  robotLost, true).
  236
  237
  238%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  239% HANDLING OF REQUESTS
  240%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  241
  242% Robot has started to get an order
  243rel_fluent(startOrder).          
  244
  245% Sender of the package holding
  246fun_fluent(sender).              
  247
  248% Recipient of the package holding
  249fun_fluent(recipient).           
  250causes_val(dropOff,    recipient, unknown,  holdingShip).
  251senses(readRecipient,  recipient).
  252
  253% The robot is holding a package
  254rel_fluent(holdingShip).         
  255causes_true(pickUp,          holdingShip, serviceAccepted(robotLocation)).
  256causes_false(dropOff,        holdingShip, true).
  257
  258% Customer requested service
  259rel_fluent(askedService(_)).     
  260causes_true(orderShipment(C,_), askedService(C), true).
  261causes_false(ackOrder(C),       askedService(C), true).
  262causes_false(declineOrder(C),   askedService(C), true).
  263
  264% Priority requested from customer
  265fun_fluent(orderPrio(_)).        
  266causes_val(orderShipment(C,P),  orderPrio(C), P, true).
  267
  268% Service was accepted for customer
  269rel_fluent(serviceAccepted(_)).        
  270causes_false(declineOrder(C), serviceAccepted(C), true).
  271causes_true(ackOrder(C),      serviceAccepted(C), true).
  272causes_false(pickUp,          serviceAccepted(C), robotLocation=C).
  273
  274% Service to customer is suspended
  275rel_fluent(suspended(_)).        
  276causes_true(suspend(C),  suspended(C), true).
  277causes_false(enable(C),  suspended(C), true).
  278
  279% Number of packages in mailbox of customer
  280fun_fluent(mailBox(_)).          
  281causes_val(emptyMailBoxes,    mailBox(C), 0, customer(C)).
  282causes_val(dropOff,           mailBox(C), N, and(robotLocation=central,
  283                                             and(recipient=C,
  284                                                 N is mailBox(C)+1)) ).
  285% The mailbox for customer is full
  286rel_fluent(mailBoxFull(_)).      
  287
  288% Customer is in his office
  289rel_fluent(inOffice(_)).         
  290causes_true(in(C),      inOffice(C), true).
  291causes_false(out(C),    inOffice(C), true).
  292senses(senseDoor(C),    inOffice(C)).
  293
  294
  295
  296
  297%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  298% LOCALIZATION
  299%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  300prim_action(startLocalization).
  301poss(startLocalization, true).
  302
  303prim_action(addNode(N)) :- domain(N,location).
  304poss(addNode(_),  robotLost).
  305
  306prim_action(addEdge(_)).
  307poss(addEdge(_),  robotLost).
  308
  309prim_action(addNEdge(_)).
  310poss(addNEdge(_), robotLost).
  311
  312prim_action(addCounter).
  313poss(addCounter, robotLost).
  314
  315prim_action(setCurrentMap(_)).
  316poss(setCurrentMap(_), robotLost).
  317
  318fun_fluent(counterStations).           
  319causes_val(startLocalization,  counterStations, 2, true).
  320causes_val(addCounter,         counterStations, X, X is counterStations+1).
  321
  322fun_fluent(currentMap).           
  323causes_val(startLocalization,  currentMap, 
  324      graph([station(1),station(2)],[edge(station(1),station(2),south),
  325	                             edge(station(2),station(1),north)],[]), true).
  326causes_val(setCurrentMap(G),   currentMap, G, true).
  327causes_val(A,                  currentMap, G, 
  328                  and(neg(A=startLocalization),
  329                  and(neg(some(g,A=setCurrentMap(g))), realWorldGraph(G))) ).
  330
  331
  332causes_val(addNode(N),  currentMap, NewMap, add_node(N,currentMap,NewMap)).
  333causes_val(addEdge(E),  currentMap, NewMap, add_edge(E,currentMap,NewMap)).
  334causes_val(addNEdge(E), currentMap, NewMap, add_nedge(E,currentMap,NewMap)).
  335
  336rel_fluent(stationVisited(_)).
  337causes_true(reachDest,           stationVisited(S), robotDestination=S).
  338causes_false(startLocalization,  stationVisited(S), location(S)).
  339
  340
  341% Check if there is a line below the path sensor (sensing action)
  342prim_action(senseLine).   
  343senses(senseLine, lineBelow).
  344poss(senseLine, true).
  345rel_fluent(lineBelow).
  346
  347proc(localize,
  348	[startLocalization,
  349         addCounter,  % Counter=3
  350	 %
  351	 while(neg(some(l,some(d,uniquePlace(l,d)))), localizeOneStep),
  352	 pi(loc,
  353	 pi(deg,
  354	 pi(dir,
  355	     [?(uniquePlace(loc,deg)),
  356	      ?(rotation(direction,dir,deg)),
  357	      setLocation(loc,dir)
  358             ]
  359           ))) 
  360        ]
  361).
  362
  363% The current map is a rotated version of the real one (it has to be rotated Deg)
  364% The robot is at location Loc in the real-world
  365proc(uniquePlace(Loc,Deg),
  366	some(realWorld,
  367	        and(realWorldGraph(realWorld),
  368                    uniqueLocation(currentMap,realWorld, robotLocation, Loc, Deg)
  369	           )
  370	     )
  371).
  372
  373proc(localizeOneStep,
  374	[discoverStation,
  375         pi(station, 
  376	 pi(path,
  377	     [?(and(getNode(station,currentMap),neg(stationVisited(station)))),
  378	      ?(path_graph_short(robotLocation,station,currentMap,10,path)),
  379	      pi(x, pi(restpath, [?(path=[x|restpath]), traversePath(restpath)]))
  380	      ])),
  381	 cleanMap]
  382).
  383
  384proc(discoverStation,
  385	[turn(-90),
  386	senseLine,
  387	if(lineBelow,addStation,addNonStation),
  388	turn(90),
  389	%
  390	turn(90),
  391	senseLine,
  392	if(lineBelow,addStation,addNonStation),
  393	turn(-90),
  394	%
  395	moveFwd,
  396	senseLine,
  397	if(lineBelow,addStation,addNonStation),
  398	moveBack]
  399).
  400
  401
  402proc(addNonStation,
  403	pi(reverseDir,
  404	   [addNEdge(edge(robotLocation,_,direction)),
  405	    ?(rotation(direction,reverseDir,180)),
  406	    addNEdge(edge(_,robotLocation,reverseDir))]
  407	    )
  408).
  409
  410proc(addStation,
  411	pi(c,
  412	pi(reverseDir,
  413	   [?(c=counterStations),
  414	    addNode(station(c)),
  415	    addEdge(edge(robotLocation,station(c),direction)),
  416	    ?(rotation(direction,reverseDir,180)),
  417	    addEdge(edge(station(c),robotLocation,reverseDir)),
  418	    addCounter]
  419	    ))
  420).
  421
  422proc(cleanMap,
  423	pi(realWorld, 
  424	pi(newMap, [?(realWorldGraph(realWorld)),
  425	            ?(cleanGraph(currentMap, realWorld, newMap)),
  426		    setCurrentMap(newMap)]
  427	  ))
  428).
  429
  430
  431
  432
  433
  434
  435% Loc is the real location of NodeG1 in graph G2 and 
  436% G1 has to be rotated Deg to fit graph G1
  437uniqueLocation(G1, G2, NodeG1, Loc, Deg) :-
  438	% First, get all possible mappings: G1->G2
  439	findall((M,D), sub_graph_rot(G2, G1, M, D), [(Map,Deg)]),
  440	member((Loc,NodeG1), Map).
  441
  442           
  443% Clean graph G1 w.r.t. graph G2 (i.e., remove known repeated nodes in G1)
  444cleanGraph(G1, G2, GNew) :-
  445	% First, get all possible mappings: G1->G2
  446	findall(M, D^sub_graph_rot(G2, G1, M, D), LMaps), 
  447	setof((Node1,Node2), (getNode(Node1,G1),  % get all Node1=Node2 in G1
  448	                      getNode(Node2,G1), 
  449		              Node1\=Node2,
  450		              \+ not_equal_nodes(Node1,Node2,LMaps)), LEqualNodes),
  451	remove_equal_nodes(LEqualNodes, G1, GNew).
  452cleanGraph(G1,_,G1).
  453
  454
  455% Node1 is not the same as Node2 if there is a Map in LMaps such that
  456% Node1 is mapped to a different node than Node2
  457not_equal_nodes(Node1, Node2, LMaps) :-
  458	member(Map, LMaps),
  459	member((MapNode1,Node1), Map),
  460	member((MapNode2,Node2), Map),
  461	MapNode1\=MapNode2.
  462
  463
  464remove_equal_nodes([], G, G).
  465remove_equal_nodes([(N1,N2)|Tail], G, GNew) :-
  466	getNode(N1,G),getNode(N2,G),!,
  467	combine_nodes(N1,N2,G,GNew2),
  468	remove_equal_nodes(Tail, GNew2, GNew).
  469remove_equal_nodes([_|Tail], G, GNew) :-
  470	remove_equal_nodes(Tail, G, GNew).
  471	
  472
  473	
  474
  475 
  476
  477
  478
  479%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  480% C - ACTIONS and PRECONDITIONS
  481%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  482
  483
  484%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  485% HANDLING OF REQUESTS
  486%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  487
  488% Acknowledge order of C
  489prim_action(ackOrder(C))      :- domain(C,office).	
  490poss(ackOrder(C), neg(serviceAccepted(C)) ).
  491
  492% Deny order to C
  493prim_action(declineOrder(C))  :- domain(C,office).   
  494poss(declineOrder(_), 	true).
  495
  496% Suspend service to C
  497prim_action(suspend(C))       :- domain(C,office).   
  498poss(suspend(C), and(neg(inOffice(C)), serviceAccepted(C)) ).
  499      
  500% Restart service to C
  501prim_action(enable(C))        :- domain(C,office).   
  502poss(enable(_), true).
  503
  504
  505
  506%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  507% ROBOT MOVEMENTS
  508%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  509
  510
  511% Pick up shipment 
  512prim_action(pickUp).                            
  513poss(pickUp, robotState=readyGo).
  514
  515% Drop shipment
  516prim_action(dropOff).                           
  517poss(dropOff, and(robotState=frozen, 
  518	          or(inOffice(robotLocation), and(robotLocation=central,
  519		                                  mailBox(recipient)<3)) 
  520	         ) 
  521).
  522
  523% Aborts navigation 
  524prim_action(resetRobot).	                
  525poss(resetRobot, true).
  526
  527% Start going to next station
  528prim_action(goNext).                            
  529poss(goNext, or(some(a,some(b,robotLocation=between(a,b))),
  530	        some(next, and(getEdge(robotLocation,next,direction,currentMap),
  531	                   neg(blocked(robotLocation,next))))) ).
  532
  533% Move back/forward a litle bit
  534prim_action(moveBack).                           
  535poss(moveBack, true).
  536
  537prim_action(moveFwd).                           
  538poss(moveFwd, true).
  539
  540% Move 90 degrees to left
  541prim_action(turnLeft).	                        
  542poss(turnLeft, isEdgeLeft).
  543
  544% Move 90 degrees to right
  545prim_action(turnRight).	                        
  546poss(turnRight, isEdgeRight).
  547
  548% Turn D degress
  549prim_action(turn(D)) :- domain(D,degree).
  550poss(turn(D), or(D=90,or(D=180,or(-90=D,-180=D))) ).
  551
  552% Move 180 degrees
  553prim_action(turnAround).                        
  554poss(turnAround, and(neg(isEdgeLeft), neg(isEdgeRight)) ).
  555
  556% Set robotLocation to L and direction to D when the robot is lost
  557prim_action(setLocation(L,D)) :- domain(L, location), domain(D, direction).
  558poss(setLocation(_,_), robotLost).
  559
  560% Do not move
  561prim_action(freezeRobot).	                
  562poss(freezeRobot, true).
  563
  564% Read the recipient of the pack that we are holding (sensing action)
  565prim_action(readRecipient).	                
  566poss(readRecipient, holdingShip).
  567
  568% Check if C is in office (sensing action)
  569prim_action(senseDoor(C)) :- domain(C,office).   
  570poss(senseDoor(C), robotLocation=C).
  571
  572% Say a message
  573prim_action(say(_)).            
  574poss(say(_), true).
  575
  576% Ring the bell
  577prim_action(ring).            
  578poss(ring, true).
  579
  580
  581%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  582% D - EXOGENOUS ACTIONS OR EVENTS
  583%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  584exog_action(debug).                    
  585
  586% Empty all mailboxes
  587exog_action(emptyMailBoxes).                    
  588
  589% Stop because totally confused
  590exog_action(stop_abnormally).                   
  591
  592% Destination reached successfully
  593exog_action(reachDest).               	        
  594
  595% Button has been pressed
  596exog_action(pushGo).               	        
  597
  598% Could not get to destination, the is blocked
  599exog_action(getStuck).                          
  600
  601% The route P1-P2 is clear
  602exog_action(clearRoute(P1,P2)) :- domain(P1, location), domain(P2, location).
  603
  604% Customer C is in/out office
  605exog_action(in(C))  :- domain(C, office).             
  606exog_action(out(C)) :- domain(C, office).   	        
  607
  608% New order has arrived from Sender with Priority
  609exog_action(orderShipment(Sender,Prio)) :- 	 
  610        domain(Sender, office), 
  611	domain(Prio, priority).
  612
  613% Talk loud or shutup
  614exog_action(talk).
  615exog_action(shutup).
  616
  617
  618
  619%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  620% E - INITIAL STATE
  621%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  622%% The robot should first acknowledge Misha and start going to his office.
  623
  624% Initially the robot is idle at the central office heading west 
  625
  626% Initial location and direction of the robot
  627initially(robotLocation, central).
  628%initially(robotLocation, station(1)).
  629initially(direction, east).
  630
  631
  632% Initial state of the robot
  633initially(robotState, idle).
  634initially(robotLost, false).
  635initially(holdingShip, false).	
  636initially(talking, true).	
  637
  638initially(startOrder,false). % No order has been started to be served

  642% Customer in office 1 is in, the other customers are "unknown"
  643initially(inOffice(central), true).  
  644
  645% Customer at office 6 (with prio 12) is the only one who has requested service 
  646initially(askedService(station(6)), true).	
  647initially(orderPrio(station(6)), 12).
  648initially(askedService(C), false) :- 
  649        domain(C,office), 
  650        \+ initially(askedService(C), true).
  651
  652% Everybody has their mailbox empty except for customer 13 with 3 packages.
  653initially(mailBox(station(13)), 3).	 
  654initially(mailBox(C), 0) :- 
  655        domain(C, office), C\=13.
  656
  657% No customer is suspended and has an accepted service
  658initially(suspended(C), false) :- domain(C,office).	
  659initially(serviceAccepted(C), false)   :- domain(C,office).
  660
  661% The initial recipient is "unknown"
  662initially(recipient,unknown).
  663
  664
  665initially(currentMap, G) :-  realWorldGraph(G).
  666initially(counterStations,5).
  667
  668
  669% Initial state of the map
  670%initially(blocked(station(4), station(14)), true).
  671%initially(blocked(station(14), station(4)), true).
  672%initially(blocked(P1,P2), false)  :- 
  673%        domain(P1, location),
  674%        domain(P2, location),
  675%        \+ initially(blocked(P1,P2), true).
  676
  677
  678
  679
  680
  681%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  682% F - ABBREVIATIONS
  683%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  684        
  685% The best customer to serve is the one that has to be served, its not
  686% currently suspended and there is no other customer to be served and not
  687% suspended with higher priority
  688proc(bestCustToServe(C), 
  689     and(serviceAccepted(C),
  690     and(neg(suspended(C)),
  691         neg(some(c,and(serviceAccepted(c),
  692                    and(neg(suspended(c)), orderPrio(c)>orderPrio(C)))))
  693        ))).
  694
  695
  696
  697% Is there an edge to the left/right of the current location and direction?
  698proc(isEdgeLeft,
  699	   some(dir, some(x, 
  700	                    and(rotation(direction,dir,-90),
  701			        getEdge(robotLocation,x,dir,currentMap))))
  702).
  703
  704proc(isEdgeRight,
  705	   some(dir, some(x, 
  706	                    and(rotation(direction,dir,90),
  707			        getEdge(robotLocation,x,dir,currentMap))))
  708).
  709
  710                      
  711
  712%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  713% G - DEFINITIONS OF COMPLEX ACTIONS
  714%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  715
  716% Execute program E in an interative deeping fashion up to Max times
  717% Stop when condition C holds
  718proc(minimization(E,C,Max), minimize(E,C,Max,0)).
  719proc(minimize(E,C,Max,N),
  720     wndet(search([exec(E,N),?(C)]), 
  721           [?(N<Max),pi(n2,[?(n2 is N+1), minimize(E,C,Max,n2)])])).
  722
  723% Execute program E exactly N consecutive times
  724proc(exec(E,N), 
  725     wndet(?(N=0), [E,pi(n2,[?(n2 is N-1),exec(E,n2)])]) ).
  726
  727% Handle a new order by accepting it if possible, otherwise decline it
  728proc(handleNewOrder(C), wndet(ackOrder(C), declineOrder(C)) ).
  729
  730% If we want to go to Dest, the next station is Next 
  731% (uses the knowledge from the Prolog clause path/3)
  732proc(nextStation(Dest,Next),
  733   ?(path_graph_short(robotLocation,Dest,currentMap,10,[_,Next|_])) ).
  734
  735% Go to Loc with the minimal number of steps
  736proc(goToLocation(Loc),
  737	pi(path,
  738	   [?(path_graph_short(robotLocation,Loc,currentMap,10,path)) ,
  739	    pi(x, pi(restpath, [?(path=[x|restpath]), traversePath(restpath)]))
  740           ])
  741).
  742
  743% Traverse a sequence of stations
  744proc(traversePath(Path),
  745	pi(next, 
  746	pi(rest, [?(Path=[next|rest]),
  747	           turnToAim(next),
  748		   goNext,
  749		   sim(reachDest),
  750		   if(rest=[], ?(true), traversePath(rest))
  751	         ]
  752	 ))
  753).
  754
  755% Turn to aim station Next
  756proc(turnToAim(Next),
  757   [star(ndet(turnLeft,ndet(turnRight,turnAround)),2), 
  758    ?(getEdge(robotLocation,Next,direction,currentMap))
  759   ]
  760).
  761
  762
  763% SERVE PROCEDURE: Serve a customer that needs service
  764% Used for both picking up a package at some office and
  765% dropping off a package that the agent is holding.
  766% serve should be used with a conditional search since it solution depend
  767% on the customer being at his office.
  768%
  769% Choose a customer that needs to be served (package to pickup or to drop-off),
  770% go to customer office, serve customer in a possible way (3 ways: pickup/drop-off,
  771% suspend, leave in mailbox), and, finally, reset to idle
  772proc(serve, 
  773pi(c,[wndet(?(and(holdingShip,c=recipient)), ?(bestCustToServe(c))),
  774      goToLocation(c), 
  775      commit, 
  776      senseDoor(c), 
  777      branch(inOffice(c)),
  778      wndet(search(service), wndet(suspend(c), [goToLocation(central), service])),
  779      resetRobot]
  780)).
  781
  782% Drop package and pick up new package
  783proc(service, [freezeRobot, dropOff, sim(pushGo), pickUp]).
  784
  785% Ask the user to reposition the robot because he is totally lost
  786proc(recover_position, 
  787	[say('I got lost! I will try to find where I am...'),
  788	 searchPath,    % search for any black line
  789	 localize
  790        ]
  791).
  792
  793prim_action(searchPath).
  794poss(searchPath, robotLost).
  795
  796% manual_localization(Location, Direction): auxiliary predicate
  797manual_localization(Location, Direction, M) :-
  798    (Direction = 1 -> MDir=' going up.' ; MDir=' going down.'),
  799    concat_atom(['I got lost heading from waystation ', Location, 
  800                 ' while ', MDir,
  801                 '.. Please position me between waystations in ',
  802                 ' the correct direction, and type any key when ready.'],M).
  803
  804%%%%%%%%%%%%%%%%%%%%%%%%%
  805%  Main Routine
  806%%%%%%%%%%%%%%%%%%%%%%%%%
  807
  808proc(mainControl(2), [prioritized_interrupts(
  809    [interrupt(or(robotState = moving, robotState = waitingPush), 
  810               wait),
  811     interrupt(true, localize)]
  812    )]
  813).
  814
  815  
  816proc(mainControl(1), [prioritized_interrupts(
  817    [interrupt(robotState = lost, 
  818               [resetRobot, recover_position, goNext]),
  819     interrupt(n, askedService(n), 
  820               handleNewOrder(n)),
  821     interrupt(or(robotState = moving, robotState = waitingPush), 
  822               wait),
  823     interrupt(robotState = stuck, 
  824               [resetRobot, 
  825                abort(startOrder), 
  826                moveBack, 
  827		turnAround, 
  828                goNext, 
  829                resetRobot]),
  830     interrupt(c, and(serviceAccepted(c), suspended(c)), 
  831               declineOrder(c)),
  832     interrupt(holdingShip, 
  833               [wndet(?(neg(recipient=unknown)), readRecipient),
  834                search([gexec(startOrder,searchc(serve))])]),
  835     interrupt(c, and(robotState = idle, serviceAccepted(c)), 
  836               [say(['Trying to serve ',c]),
  837                wndet(search([gexec(startOrder, searchc(serve))]), 
  838                      [say(['Sorry it is not safe to serve ',c]),declineOrder(c)])
  839               ]), 
  840     interrupt(and(neg(robotLocation=central), neg(holdingShip)), 
  841               [say('Wrapping up to central office...'),
  842                search(pi(c,[nextStation(central,c), turnToAim(c), goNext])),
  843                resetRobot]), 
  844     interrupt(true, [say('Waiting at central station....'), wait])]
  845     )]).
  846
  847
  848
  849%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  850% H - ACTION/MESSAGE MAPPINGS - NUMBERS MUST CORRESPOND TO NQC CODE
  851%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  852
  853% actionNum(?Action, ?ActionNumber): Returns ActionNumber associated
  854%     with Action and vice versa. ActionNumber can be sent to the RCX
  855%     for execution of Action. It can be returned from the RCX to
  856%     report the occurrence of exogenous Action
  857
  858% This action have no impact on the RCX
  859%% actionNum(readRecipient, 0).
  860%% actionNum(enable(_), 0).
  861%% actionNum(suspend(_), 0).
  862%% actionNum(declineOrder(_), 0).
  863%% actionNum(ackOrder(_), 0).
  864%% actionNum(recover_position, 0).
  865
  866% These actions should be sent to the RCX for action
  867actionNum(turnAround,   1).
  868actionNum(turnLeft,     2).
  869actionNum(turnRight,    3).
  870actionNum(pickUp,       4).
  871actionNum(dropOff,      5).
  872actionNum(goNext,       6).
  873actionNum(moveBack,     7).
  874actionNum(moveFwd,      8).
  875actionNum(freezeRobot,  9).
  876%actionNum(senseDoor(_), 10).
  877actionNum(resetRobot,   11).
  878actionNum(ring,         12).
  879actionNum(senseLine,    13).
  880actionNum(searchPath,   14).
  881
  882% Exogenous actions
  883actionNum(reachDest,       20).
  884actionNum(stop_abnormally, 21).
  885actionNum(pushGo,          22).
  886actionNum(getStuck,        23).
  887
  888
  889
  890%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  891% I - Translation of sensor values from RCX
  892%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  893
  894% translateSensorValue(+Action, +SensorValue, SensingResult): Translate
  895%     the value SensorValue returned by the RCX sensor into a legal
  896%     SensingResult under Action
  897
  898%translateSensorValue(A, SensorValue, SensorResult):- 
  899%	A=senseDoor(_), 
  900%	SensorValue>25-> SensorResult=true ; SensorResult=false.
  901%translateSensorValue(_, SensorValue, SensorValue).  % For all the other actions
  902
  903
  904
  905
  906
  907
  908%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  909% J - OTHERS
  910%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  911
  912%:- include('map-circle').   % Include the map
  913
  914
  915
  916
  917%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  918% MAP AND PATH TOOLS
  919%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  920
  921% sub_graph_rot(G1, G2, Map): 
  922%   the possibly incomplete graph G2 is a rotated subgraph of G1 under mapping Map
  923sub_graph_rot(G1, G2, Map, D) :-
  924	member(D,[0,90,180,270]),
  925	rotate_graph(G2, D, RG2),
  926	sub_graph(G1,RG2,Map).
  927
  928% rotate_graph(G1, D, G2): graph G2 is graph G1 rotated D degress clockwise
  929rotate_graph(graph(Nodes, Edges), D, graph(RNodes, REdges)) :-
  930	rotate_graph(graph(Nodes, Edges,[]), D, graph(RNodes, REdges,[])).
  931rotate_graph(graph(Nodes, Edges, NEdges), D, graph(Nodes, REdges,RNEdges)) :-
  932	maplist(rotate_edge(D),Edges,REdges),
  933	maplist(rotate_edge(D),NEdges,RNEdges).
  934
  935% Rotate the orientation of an edge some Degrees
  936rotate_edge(Degrees, edge(S,D,O), edge(S,D,RO)) :-
  937	rotation(O, RO, Degrees).
  938
  939% rotation(X,Y,D) : Y is D clockwise degrees from X
  940rotation(X, Y, D) :-
  941	rotate_clock(X,Y,DC),
  942	(D=DC ; D is (360-DC)*(-1)).
  943
  944rotate_clock(X, X, 0).
  945rotate_clock(X, Y, 90) :-
  946	rot(X, Y, 90).
  947rotate_clock(X, Y, 180) :-
  948	rot(X, Z, 90),
  949	rot(Z, Y, 90).
  950rotate_clock(X, Y, 270) :-
  951	rot(X, Z, 90),
  952	rot(Z, W, 90),
  953	rot(W, Y, 90).
  954
  955rot(north,east,90).
  956rot(east,south,90).
  957rot(south,west,90).
  958rot(west,north,90).
  959
  960
  961
  962%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  963% DEFINITION OF THE REAL WORLD GRAPH
  964%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  965
  966% edge definition for the map CIRCLE
  967nodes(circle,L)    :- location(L).
  968edge(circle,X,Y,D) :- edge1(circle,X,Y,D). 
  969edge(circle,X,Y,D) :- rotation(D2,D,180), edge1(circle,Y,X,D2).
  970
  971edge1(circle, central,	        station(2),     north).
  972edge1(circle, station(2),	station(3),     east).
  973edge1(circle, station(3),	station(4),     south).
  974edge1(circle, station(4),	station(5),     south).
  975edge1(circle, central,	        station(15),    south).
  976edge1(circle, station(15),	station(5),     east).
  977
  978edge1(circle, station(2),	station(6),	west).
  979
  980edge1(circle, central,	        station(8),     west).
  981edge1(circle, station(8),	station(7),     north).
  982edge1(circle, station(8),	station(9),     south).
  983
  984edge1(circle, station(3),	station(10),	east).
  985edge1(circle, station(10),	station(11),	east).
  986edge1(circle, station(10),	station(14),	south).
  987edge1(circle, station(14),	station(13),	east).
  988edge1(circle, station(13),	station(12),	north).
  989edge1(circle, station(13),	station(16),	south).
  990edge1(circle, station(16),	station(5),	west).
  991
  992
  993/* World topology dictated by edge/3:
  994
  995
  996     6 ------------2------------3---------10---------11
  997                   |            |         |
  998                   |            |         |
  999                   |            |         |
 1000                   |            |         |
 1001     7             |            |         |          12
 1002     |             |            |         |          |
 1003     |             |            |         |          | 
 1004     |             |            |         14---------13
 1005     |             |            |         |          |
 1006     |             |            |         |          |
 1007     8 ---------central         |         |          |
 1008     |             |            |         |          |
 1009     |             |            |         |          |
 1010     |             |            4---------|          |
 1011     |             |            |                    |
 1012     |             |            |                    |
 1013     |            15------------5--------------------16
 1014     |                                             
 1015     |                                             
 1016     9             
 1017|*/
 1018
 1019
 1020map_graph(Id, graph(Nodes, Edges)) :-
 1021	nodes(Id, Nodes),
 1022	setof(edge(S,D,O),O2^(edge(Id,S,D,O) ; 
 1023                              edge(Id,D,S,O2), rotation(O2,O,180)),Edges).
 1024
 1025realWorldGraph(G) :- map_graph(circle, G).
 1026
 1027
 1028
 1029%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 1030% OLDDDDDDDDDDDDDDD
 1031%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 1032
 1033% Generic facts about connectivity: 
 1034% connected(Id, X, Y, D):-
 1035%              X and Y are connected with Y at direction D to X in Id
 1036connected(Id, between(P1,P2),P,D):- !,
 1037	( connected(Id, P1,P2,D), P=P2 
 1038	; 
 1039	  connected(Id, P2,P1,D),P=P1 
 1040	).
 1041connected(Id, X, Y, west) :- edge(Id, X, Y, west)  ; edge(Id, Y, X, east).
 1042connected(Id, X, Y, east) :- edge(Id, X, Y, east)  ; edge(Id, Y, X, west).
 1043connected(Id, X, Y, north):- edge(Id, X, Y, north) ; edge(Id, Y, X, south).
 1044connected(Id, X, Y, south):- edge(Id, X, Y, south) ; edge(Id, Y, X, north).
 1045
 1046% C1 and C2 are connected in some way
 1047connected(Id, C1, C2):- connected(Id, C1, C2, _) ; connected(Id, C2, C1, _).
 1048
 1049
 1050% hasLeft(X,D) : at X there is a route on the left when aiming D
 1051% hasRight(X,D): at X there is a route on the right when aiming D
 1052hasLeft(Id, X, D) :- rotation(D, D2, -90), connected(Id, X,_, D2).
 1053hasRight(Id, X, D):- rotation(D, D2, 90),  connected(Id, X,_, D2).
 1054
 1055
 1056% Dist is the minimal distance between P1 and P2
 1057mindist(P1, P2, Dist) :- mindist2(P1,P2,Dist,1).
 1058
 1059mindist2(P1,P2,Limit,Limit) :- length(Path,Limit), path(P1,P2,Path), !.
 1060mindist2(P1,P2,Dist,Limit)  :- L2 is Limit+1,  mindist2(P1,P2,Dist,L2).
 1061
 1062
 1063% path(X, Y, Path): Path is a list of locations to pass through
 1064%   in order to get from X to Y
 1065
 1066% We'll strip off the first element as it will be X
 1067path(X, Y, Path) :- path1(X, [Y], [X|Path]). 
 1068
 1069path1(X, [X | Path], [X | Path]).
 1070
 1071path1(X, [Y | Path1], Path) :-
 1072    connected(Y, Z, _),
 1073    \+ member(Z, Path1),
 1074    path1(X, [Z, Y | Path1], Path).
 1075
 1076% in_path(X, Y, Z): Z is the next element in a path from X to Y
 1077in_path(X, Y, Z) :-
 1078	path(X, Y, [Z|_]).
 1079
 1080
 1081% P is the shortest path in G from X to Y (and with length less than Limit)
 1082path_plan_short(X, Y, G, Limit, P) :- path_plan_short(X,Y,G,0,Limit, P).
 1083
 1084path_plan_short(X,Y,G,N,_,P) :- path_plan(X,Y,G,N,P), !.
 1085path_plan_short(X,Y,G,N,L,P) :-
 1086	L\=0,
 1087	L2 is L-1, N2 is N+1, path_plan_short(X,Y,G,N2,L2,P).
 1088
 1089
 1090% path_graph(X, Y, Id, L, P): P is a path of length L from X to Y in map Id
 1091path_plan(X, Y, Id, L, [X|LV]) :- 
 1092	length(LV, L), % Build a list LV of variables of length L
 1093	path1_plan(Id, Y, [X|LV]). 
 1094
 1095path1_plan(_, X, [X]).
 1096path1_plan(Id, X, [Y | Path1]) :- 
 1097	edge(Id,Z,Y,_),  % We can go from Y to Z with an edge in E
 1098%	append(GList, VList, Path1),
 1099%	ground(GList), 
 1100%	\+ member(Z, Path1),
 1101	Path1=[Z|_],
 1102	path1_plan(Id, X, Path1).
 1103
 1104
 1105
 1106
 1107%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 1108% EOF: Delivery-BAT/delivery.pl
 1109%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 1110
 1111/*
 1112
 11134 ?-  now(H), trans(searchc(serve),H,E,H).
 1114
 1115Action (h for help) ? abort
 1116% Execution Aborted
 11175 ?- now(H), trans(search(goToLocation(station(12))),H,E,R).
 1118
 1119H = [e(readRecipient, station(12)), readRecipient, pickUp, pushGo, dropOff, freezeRobot,
 1120e(senseDoor(station(...)), true), senseDoor(station(...)), reachDest|...]
 1121E = followpath([[], pi(x, pi(restpath, [? ([station(...)|...]=[x|...]), traversePath(restpath)]))], [[[], pi(x, pi(restpath, [? ([...|...]=[...|...]), traversePath(restpath)]))], [e(readRecipient, station(12)), readRecipient, pickUp, pushGo, dropOff, freezeRobot|...], [[[], traversePath([station(...)|...])]], [e(readRecipient, station(12)), readRecipient, pickUp, pushGo|...], [[[[]|...]]], [e(readRecipient, station(...)), readRecipient|...], [[...]], [...|...]|...])
 1122R = [e(readRecipient, station(12)), readRecipient, pickUp, pushGo, dropOff, freezeRobot,
 1123e(senseDoor(station(...)), true), senseDoor(station(...)), reachDest|...]
 1124
 1125Yes
 1126
 1127
 1128?- now(H).
 1129
 1130H = [e(readRecipient, station(12)), readRecipient, pickUp, pushGo, dropOff, freezeRobot, e(senseDoor(station(...)), true), senseDoor(station(...)), reachDest|...] [write]
 1131
 1132H = [e(readRecipient, station(12)), readRecipient, pickUp, pushGo, dropOff, freezeRobot, e(senseDoor(station(6)), true), senseDoor(station(6)), reachDest, goNext, turnLeft, reachDest, goNext, turnLeft, e(startOrder, true), say(['Trying to serve ', station(6)]), ackOrder(station(6))]
 1133
 1134
 1135assert(now([e(readRecipient, station(12)), readRecipient, pickUp, pushGo, dropOff, freezeRobot, e(senseDoor(station(6)), true), senseDoor(station(6)), reachDest, goNext, turnLeft, reachDest, goNext, turnLeft, e(startOrder, true), say(['Trying to serve ', station(6)]), ackOrder(station(6))])).
 1136
 1137
 1138assert(graph(graph([station(1), station(2), station(3)], [edge(station(1), station(2), south), edge(station(2), station(1), north), edge(station(2), station(3), south), edge(station(3), station(2), north)], [])),1).
 1139
 1140
 1141assert(graph(graph([station(1), station(2), station(3), station(4)], [edge(station(4), station(2), south), edge(station(2), station(4), north), edge(station(1), station(2), south), edge(station(2), station(1), north), edge(station(2), station(3), south), edge(station(3), station(2), north)], []),2)).
 1142
 1143*/