[PAIP] Upload GPS and move on
This commit is contained in:
parent
0c84539e60
commit
6c0ea3cc33
|
|
@ -0,0 +1,559 @@
|
|||
(defvar *dbg-ids* nil "Identifiers used by dbg.")
|
||||
|
||||
(defun debug-pseudo (&rest ids)
|
||||
"Start dbg output on the given ids."
|
||||
(setf *dbg-ids* (union ids *dbg-ids*)))
|
||||
|
||||
(defun undebug-pseudo (&rest ids)
|
||||
"Stop dbg on the ids. With no ids, stop dbg altogether."
|
||||
(setf *dbg-ids* (unless (null ids) (set-difference *dbg-ids* ids))))
|
||||
|
||||
(defun dbg (id indent format-string &rest args)
|
||||
"Print debugging info if (DEBUG ID) has been specified."
|
||||
(when (member id *dbg-ids*)
|
||||
(format *debug-io* "~&~V@T~?" (* 2 indent) format-string args)))
|
||||
|
||||
(defvar *ops* nil "A list of available operators.")
|
||||
|
||||
(defstruct op
|
||||
"An operation"
|
||||
(action nil)
|
||||
(preconds nil)
|
||||
(add-list nil)
|
||||
(del-list nil))
|
||||
|
||||
(defun starts-with? (list x)
|
||||
"Is this a list whose first element is x?"
|
||||
(and (consp list) (eql (first list) x)))
|
||||
|
||||
(defun executing? (x)
|
||||
"Is x of the form: (executing ...) ?"
|
||||
(starts-with? x 'executing))
|
||||
(defun action? (state)
|
||||
"Is state an action?"
|
||||
(or (equal state '(start)) (executing? x)))
|
||||
|
||||
(defun convert-op! (op)
|
||||
"Make op conform to the (EXECUTING op) convention."
|
||||
(unless (some #'executing? (op-add-list op))
|
||||
(push (list 'executing (op-action op)) (op-add-list op)))
|
||||
op)
|
||||
|
||||
(defun op (action &key preconds add-list del-list)
|
||||
(convert-op! (make-op :action action
|
||||
:preconds preconds
|
||||
:add-list add-list
|
||||
:del-list del-list)))
|
||||
|
||||
(defun appropriate? (goal op)
|
||||
"An op is appropriate to a goal if it's in its add-list."
|
||||
(member goal (op-add-list op) :test #'equal))
|
||||
|
||||
(defun apply-op (state goal op goal-stack)
|
||||
"Return a new, transformed state if op is applicable."
|
||||
(dbg 'gps (length goal-stack) "; Consider: ~a" (op-action op))
|
||||
(let ((state2 (achieve-all state (op-preconds op) (cons goal goal-stack))))
|
||||
(unless (null state2)
|
||||
(dbg 'gps (length goal-stack) "; Action: ~a" (op-action op))
|
||||
(append (remove-if (lambda (x) (member x (op-del-list op) :test #'equal))
|
||||
state2)
|
||||
(op-add-list op)))))
|
||||
|
||||
(defun appropriate-ops (goal state)
|
||||
"Return a list of appropriate operators,
|
||||
sorted by the number of unfulfulled preconditions."
|
||||
(sort (copy-list (remove-if-not (lambda (op) (appropriate? goal op)) *ops*))
|
||||
#'< :key (lambda (op)
|
||||
(count-if (lambda (precond)
|
||||
(not (member precond state :test #'equal)))
|
||||
(op-preconds op)))))
|
||||
|
||||
(defun achieve (state goal goal-stack)
|
||||
"A goal is achieved if it already holds,
|
||||
or if there is an appropriate op for it that is applicable."
|
||||
(dbg 'gps (length goal-stack) "; Goal: ~a" goal)
|
||||
(cond ((member goal state :test #'equal) state)
|
||||
((member goal goal-stack :test #'equal) nil) ; in case of infinite loop
|
||||
(t (some (lambda (op) (apply-op state goal op goal-stack))
|
||||
(appropriate-ops goal state)))))
|
||||
|
||||
(defun achieve-all (state goals goal-stack)
|
||||
"Try to achieve each goal, then make sure they still hold."
|
||||
(let ((current-state state))
|
||||
(if (and (every (lambda (g) (setf current-state
|
||||
(achieve current-state g goal-stack)))
|
||||
goals)
|
||||
(subsetp goals current-state :test #'equal))
|
||||
current-state)))
|
||||
|
||||
(defun GPS (state goals &optional (*ops* *ops*))
|
||||
"General Problem Solver: achieve all goals from current *state* using *ops*."
|
||||
(remove-if-not #'action? (achieve-all (cons '(start) state) goals nil)))
|
||||
|
||||
(defun use! (oplist)
|
||||
"Use oplist as the default list of operators."
|
||||
(length (setf *ops* oplist)))
|
||||
|
||||
; Drive son to school
|
||||
(defparameter *school-ops*
|
||||
(list
|
||||
(make-op :action 'drive-son-to-school
|
||||
:preconds '(son-at-home car-works)
|
||||
:add-list '(son-at-school)
|
||||
:del-list '(son-at-home))
|
||||
(make-op :action 'shop-installs-battery
|
||||
:preconds '(car-needs-battery shop-knows-problem shop-has-money)
|
||||
:add-list '(car-works))
|
||||
(make-op :action 'tell-shop-problem
|
||||
:preconds '(in-comunication-with-shop)
|
||||
:add-list '(shop-knows-problem))
|
||||
(make-op :action 'telephone-shop
|
||||
:preconds '(know-phone-number)
|
||||
:add-list '(in-comunication-with-shop))
|
||||
(make-op :action 'ask-phone-number
|
||||
:preconds '(in-comunication-with-shop)
|
||||
:add-list '(know-phone-number))
|
||||
(make-op :action 'look-up-number
|
||||
:preconds '(have-phone-book)
|
||||
:add-list '(know-phone-number))
|
||||
(make-op :action 'give-shop-money
|
||||
:preconds '(have-money)
|
||||
:add-list '(shop-has-money)
|
||||
:del-list '(have-money))))
|
||||
(mapc #'convert-op! *school-ops*)
|
||||
|
||||
; Monkey and Bananas
|
||||
(defparameter *banana-ops*
|
||||
(list (op 'climb-on-chair
|
||||
:preconds '(chair-at-middle-room at-middle-room on-floor)
|
||||
:add-list '(at-bananas on-chair)
|
||||
:del-list '(at-middle-room on-chair))
|
||||
(op 'push-chair-from-door-to-middle-room
|
||||
:preconds '(chair-at-door at-door)
|
||||
:add-list '(chair-at-middle-room at-middle-room)
|
||||
:del-list '(chair-at-door at-door))
|
||||
(op 'walk-from-door-to-middle-room
|
||||
:preconds '(at-door on-floor)
|
||||
:add-list '(at-middle-room)
|
||||
:del-list '(at-door))
|
||||
(op 'grasp-bananas
|
||||
:preconds '(at-bananas empty-handed)
|
||||
:add-list '(has-bananas)
|
||||
:del-list '(empty-handed))
|
||||
(op 'drop-ball
|
||||
:preconds '(has-ball)
|
||||
:add-list '(empty-handed)
|
||||
:del-list '(has-ball))
|
||||
(op 'eat-bananas
|
||||
:preconds '(has-bananas)
|
||||
:add-list '(empty-handed not-hungry)
|
||||
:del-list '(has-bananas hungry))))
|
||||
|
||||
; Maze searching
|
||||
(defun make-maze-op (src dest)
|
||||
"Make an operator to more between two places."
|
||||
(op `(move from ,src to ,dest)
|
||||
:preconds `((at ,src))
|
||||
:add-list `((at ,dest))
|
||||
:del-list `((at ,src))))
|
||||
|
||||
(defun make-maze-ops (pairs)
|
||||
"Make maze ops from list of directions."
|
||||
(unless (null pairs)
|
||||
(let ((pair (first pairs)))
|
||||
(list* (make-maze-op (car pair) (cdr pair))
|
||||
(make-maze-op (cdr pair) (car pair))
|
||||
(make-maze-ops (rest pairs))))))
|
||||
|
||||
(defparameter *maze-ops*
|
||||
(make-maze-ops
|
||||
'((1 . 2) (2 . 3) (3 . 4) (4 . 9) (9 . 14) (9 . 8) (8 . 7)
|
||||
(7 . 12) (12 . 13) (12 . 11) (11 . 6) (11 . 16) (16 . 17)
|
||||
(17 . 22) (21 . 22) (22 . 23) (23 . 18) (23 . 24) (24 . 19)
|
||||
(19 . 20) (20 . 15) (15 . 10) (10 . 5) (20 . 25))))
|
||||
|
||||
(defun find-path (start end)
|
||||
"Search the maze for a path from start to end."
|
||||
(let ((result (GPS `((at ,start)) `((at ,end)) *maze-ops*)))
|
||||
(unless (null result)
|
||||
(cons start (mapcar (lambda (action) (fifth (second action)))
|
||||
(rest result))))))
|
||||
|
||||
; Blocks World
|
||||
(defun move-op (a b c)
|
||||
"Make an operator to move a from b to c."
|
||||
(labels ((move-ons (a b c)
|
||||
(if (eq b 'table)
|
||||
`((,a on ,c))
|
||||
`((,a on ,c) (space on ,b)))))
|
||||
(op `(move ,a from ,b to ,c)
|
||||
:preconds `((space on ,a) (space on ,c) (,a on ,b))
|
||||
:add-list (move-ons a b c)
|
||||
:del-list (move-ons a c b))))
|
||||
|
||||
(defun make-block-ops (blocks)
|
||||
"Create an operator for each possible block move."
|
||||
(let ((ops nil))
|
||||
(dolist (a blocks)
|
||||
(dolist (b blocks)
|
||||
(unless (equal b a)
|
||||
(push (move-op a 'table b) ops)
|
||||
(push (move-op a b 'table) ops)
|
||||
(dolist (c blocks)
|
||||
(unless (or (equal c a) (equal b c))
|
||||
(push (move-op a b c) ops))))))
|
||||
ops))
|
||||
|
||||
(defun range (start stop &optional (step 1))
|
||||
"Return a list of numbers from start (inclusive) to stop (exclusive) by step."
|
||||
(let (result)
|
||||
(do ((a start (+ a step)))
|
||||
((>= (* (- a stop) step) 0) (reverse result))
|
||||
(push a result))))
|
||||
(defun permutations (list &optional r)
|
||||
"Return successive r-length permutations of elements in the list."
|
||||
(let* ((n (length list))
|
||||
(r (or r n)))
|
||||
(unless (> r n)
|
||||
(let ((indices (range 0 n))
|
||||
(cycles (range n (- n r) -1))
|
||||
(rrr (range (1- r) -1 -1))
|
||||
(result (list (subseq list 0 r))))
|
||||
(do ((going nil (dolist (i rrr t)
|
||||
(mapc #'print `(,i ,indices ,cycles ,result))
|
||||
(decf (elt cycles i))
|
||||
(let ((j (elt cycles i)))
|
||||
(cond ((= j 0)
|
||||
(setf (elt cycles i) (- n i))
|
||||
(let ((tail (elt indices i)))
|
||||
(nconc (delete tail indices) (list tail))))
|
||||
(t (rotatef (elt indices (- n j))
|
||||
(elt indices i))
|
||||
(push (mapcar (lambda (k) (elt list k))
|
||||
(subseq indices 0 r))
|
||||
result)
|
||||
(return)))))))
|
||||
(going (reverse result)))))))
|
||||
|
||||
; Searching tools
|
||||
(defconstant fail nil "Indication of search failure.")
|
||||
(defun tree-search (states goal? successors combiner)
|
||||
"Find a state that satisfies goal?. Start with states,
|
||||
and search according to successors and combiner."
|
||||
(dbg 'search 0 ";; Search: ~a" states)
|
||||
(cond ((null states) fail)
|
||||
((funcall goal? (first states)) (first states))
|
||||
(t (tree-search (funcall combiner
|
||||
(funcall successors (first states))
|
||||
(rest states))
|
||||
goal? successors combiner))))
|
||||
(defun tree-do-search (states goal? successors combiner)
|
||||
"Find a state that satisfies goal?. Start with states, and search according to
|
||||
successors and combiner. Use an explicit loop rather than recursion."
|
||||
(if (null states)
|
||||
fail
|
||||
(do ((current-states states
|
||||
(funcall combiner
|
||||
(funcall successors (first current-states))
|
||||
(rest current-states))))
|
||||
((null current-states) fail)
|
||||
(dbg 'search 0 ";; Search: ~a" current-states)
|
||||
(when (funcall goal? (first current-states))
|
||||
(return (first current-states))))))
|
||||
|
||||
(defun prepend (new old) (append old new))
|
||||
(defun is (value &key (key #'identity) (test #'eql))
|
||||
(lambda (x) (funcall test (funcall key x) value)))
|
||||
|
||||
(defun dfs (start goal? successors)
|
||||
"Search new states first until goal is reached."
|
||||
(tree-search (list start) goal? successors #'append))
|
||||
(defun bfs (start goal? successors)
|
||||
"Search old states first until goal is reached."
|
||||
(tree-search (list start) goal? successors #'prepend))
|
||||
|
||||
(defun binary-tree (x) (list (* x 2) (+ x x 1)))
|
||||
(defun finite-binary-tree (n)
|
||||
"Return a successor function that generates a binary tree with n nodes."
|
||||
(lambda (x) (remove-if (lambda (child) (> child n))
|
||||
(binary-tree x))))
|
||||
(defun next2 (x) (list (+ x 1) (+ x 2)))
|
||||
|
||||
(defun sorter (cost-fn)
|
||||
"Return a combiner function that sorts according to cost-fn."
|
||||
(lambda (new old)
|
||||
(merge 'list (sort new #'< :key cost-fn) old #'< :key cost-fn)))
|
||||
(defun best-1st-search (start goal? successors cost-fn)
|
||||
"Search the lowest cost states first until goal is reached."
|
||||
(tree-search (list start) goal? successors (sorter cost-fn)))
|
||||
|
||||
(defun diff (num)
|
||||
"Return the function that finds the difference from num."
|
||||
(lambda (x) (abs (- x num))))
|
||||
(defun price-is-right (price)
|
||||
"Return a function that measures the difference from price,
|
||||
but give a penalty for going over the price."
|
||||
(lambda (x) (if (> x price)
|
||||
most-positive-fixnum
|
||||
(- price x))))
|
||||
|
||||
(defun beam-search (start goal? successors cost-fn beam-width)
|
||||
"Search highest scoring states first until goal is reached,
|
||||
but never consider more than beam-width states at a time."
|
||||
(tree-search (list start) goal? successors
|
||||
(lambda (new old)
|
||||
(let ((sorted (funcall (sorter cost-fn) new old)))
|
||||
(if (<= (length sorted) beam-width)
|
||||
sorted
|
||||
(subseq sorted 0 beam-width))))))
|
||||
(defun iter-wide-search (start goal? successors cost-fn
|
||||
&key (width 1) (max 100))
|
||||
"Search, increasing beam width from width to max.
|
||||
Return the first solution found at any width."
|
||||
(dbg 'search 0 "; Width: ~d" width)
|
||||
(unless (> width max)
|
||||
(or (beam-search start goal? successors cost-fn width)
|
||||
(iter-wide-search start goal? successors cost-fn
|
||||
:width (1+ width) :max max))))
|
||||
(defun search-all (start goal? successors cost-fn beam-width)
|
||||
"Find all solutions to a search problem, using beam search."
|
||||
(let ((solutions nil))
|
||||
(beam-search start
|
||||
(lambda (x) (when (funcall goal? x) (push x solutions)) nil)
|
||||
successors cost-fn beam-width)
|
||||
solutions))
|
||||
|
||||
(defstruct (city (:type list)) name long lat)
|
||||
(defparameter *cities*
|
||||
'((Atlanta 84.23 33.45)
|
||||
(Boston 71.05 42.21)
|
||||
(Chicago 87.37 41.50)
|
||||
(Denver 105.00 39.45)
|
||||
(Eugene 123.05 44.03)
|
||||
(Flagstaff 111.41 35.13)
|
||||
(Grand-Jet 108.37 39.05)
|
||||
(Houston 105.00 34.00)
|
||||
(Indianapolis 86.10 39.46)
|
||||
(Jacksonville 81.40 30.22)
|
||||
(Kansas-City 94.35 39.06)
|
||||
(Los-Angeles 118.15 34.03)
|
||||
(Memphis 90.03 35.09)
|
||||
(New-York 73.58 40.47)
|
||||
(Oklahoma-City 97.28 35.26)
|
||||
(Pittsburgh 79.57 40.27)
|
||||
(Quebec 71.11 46.49)
|
||||
(Reno 119.49 39.30)
|
||||
(San-Francisco 122.26 37.47)
|
||||
(Tampa 82.27 27.57)
|
||||
(Victoria 123.21 48.25)
|
||||
(Wilmington 77.57 34.14)))
|
||||
|
||||
(defconstant earth-diameter 12765.0 "Diameter of planet Earth in kilometers.")
|
||||
(defun distance (point1 point2)
|
||||
"The Euclidean distance between two points.
|
||||
The points are coordinates in n-dimensional space."
|
||||
(sqrt (reduce #'+ (mapcar (lambda (a b) (expt (- a b) 2)) point1 point2))))
|
||||
(defun deg->rad (deg)
|
||||
"Convert degrees and minutes to radians."
|
||||
(* (+ (truncate deg) (* (rem deg 1) 100/60)) pi 1/180))
|
||||
(defun xyz-coords (city)
|
||||
"Return the (x y z) coordinates of a point on a sphere.
|
||||
The center is (0 0 0) and the north pole is (0 0 1)."
|
||||
(let ((psi (deg->rad (city-lat city)))
|
||||
(phi (deg->rad (city-long city))))
|
||||
(list (* (cos psi) (cos phi))
|
||||
(* (cos psi) (sin phi))
|
||||
(sin psi))))
|
||||
(defun air-distance (city1 city2)
|
||||
"The great circle distance between two cities."
|
||||
(let ((d (distance (xyz-coords city1) (xyz-coords city2))))
|
||||
(* earth-diameter (asin (/ d 2)))))
|
||||
|
||||
(defun neighbor-cities (city)
|
||||
"Find all cities within 1000 kilometers."
|
||||
(remove-if (lambda (c) (or (eq c city) (> (air-distance c city) 1000.0)))
|
||||
*cities*))
|
||||
(defun city (name) "Find the city with this name." (assoc name *cities*))
|
||||
|
||||
(defstruct (path (:print-function print-path))
|
||||
state (previous nil) (cost-so-far 0) (total-cost 0))
|
||||
(defun print-path (path &optional (stream t) depth)
|
||||
(declare (ignore depth))
|
||||
(format stream "#<Path to ~a costs ~,1f>"
|
||||
(path-state path) (path-total-cost path)))
|
||||
(defun map-path (fn path)
|
||||
"Call fn on each state in the path, collecting the results."
|
||||
(unless (null path)
|
||||
(cons (funcall fn (path-state path))
|
||||
(map-path fn (path-previous path)))))
|
||||
(defun show-city-path (path &optional (stream t))
|
||||
"Show the length of a path, and the cities along it."
|
||||
(format stream "#<Path ~,1f km: ~{~:(~a~)~^ - ~}>"
|
||||
(path-total-cost path) (reverse (map-path #'city-name path)))
|
||||
(values))
|
||||
|
||||
(defun path-saver (successors cost-fn cost-left-fn)
|
||||
(lambda (old-path)
|
||||
(let ((old-state (path-state old-path)))
|
||||
(mapcar (lambda (new-state)
|
||||
(let ((old-cost (+ (path-cost-so-far old-path)
|
||||
(funcall cost-fn old-state new-state))))
|
||||
(make-path :state new-state
|
||||
:previous old-path
|
||||
:cost-so-far old-cost
|
||||
:total-cost (+ (funcall cost-left-fn new-state)
|
||||
old-cost))))
|
||||
(funcall successors old-state)))))
|
||||
(defun trip (start dest &optional (beam-width 1))
|
||||
"Search for a way from the start to dest."
|
||||
(beam-search (make-path :state start)
|
||||
(is dest :key #'path-state)
|
||||
(path-saver #'neighbor-cities #'air-distance
|
||||
(lambda (c) (air-distance c dest)))
|
||||
#'path-total-cost
|
||||
beam-width))
|
||||
|
||||
(defun new-states (states successors state= old-states)
|
||||
"Generate successor states that have not been seen before."
|
||||
(remove-if (lambda (state) (or (member state old-states :test state=)
|
||||
(member state states :test state=)))
|
||||
(funcall successors (first states))))
|
||||
(defun graph-search (states goal? successors combiner
|
||||
&optional (state= #'eql) old-states)
|
||||
"Find a state that satisfies goal?. Start with states and search
|
||||
according to successors and combiner. Don't try the same state twice."
|
||||
(dbg 'search 0 ";; Search: ~a" states)
|
||||
(cond ((null states) fail)
|
||||
((funcall goal? (first states)) (first states))
|
||||
(t (graph-search (funcall combiner
|
||||
(new-states states successors state= old-states)
|
||||
(rest states))
|
||||
goal? successors combiner state=
|
||||
(adjoin (first states) old-states :test state=)))))
|
||||
(defun hash-graph-search (states goal? successors combiner
|
||||
&optional (state= #'eql)
|
||||
(old-states
|
||||
(let ((os (make-hash-table :test state=)))
|
||||
(mapcar (lambda (state)
|
||||
(setf (gethash state os) t))
|
||||
states)
|
||||
os)))
|
||||
"Find a state that satisfies goal?. Start with states and search according to
|
||||
successors and combiner. Use hash-table to test and avoid states that
|
||||
has been seen before."
|
||||
(dbg 'search 0 ";; Search: ~a" states)
|
||||
(cond ((null states) fail)
|
||||
((funcall goal? (first states)) (first states))
|
||||
(t (setf (gethash (first states) old-states) t)
|
||||
(hash-graph-search
|
||||
(funcall combiner
|
||||
(remove-if-not
|
||||
(lambda (state) (unless (gethash state old-states)
|
||||
(setf (gethash state old-states) t)))
|
||||
(funcall successors (first states)))
|
||||
(rest states))
|
||||
goal? successors combiner state= old-states))))
|
||||
|
||||
(defun path-states (path)
|
||||
"Collect the states along this path."
|
||||
(map-path #'identity path))
|
||||
(defun find-path (state paths state=)
|
||||
"Find the path with this state among a list of paths."
|
||||
(find state paths :key #'path-state :test state=))
|
||||
(defun better-path? (path1 path2)
|
||||
"Is path1 cheaper than path2?"
|
||||
(< (path-total-cost path1) (path-total-cost path2)))
|
||||
(defun insert-path (path paths)
|
||||
"Put path into the right position, sorted by total cost."
|
||||
(merge 'list (list path) paths #'< :key #'path-total-cost))
|
||||
(defun a*-search (paths goal? successors cost-fn cost-left-fn
|
||||
&optional (state= #'eql) old-paths)
|
||||
"Find a path whose state satisfies goal?. Start with paths,
|
||||
and expand successors, exploring least cost first.
|
||||
When there are duplicate states, keep the one with the lower cost
|
||||
and discard the other."
|
||||
(dbg 'search 0 ";; Search: ~a" paths)
|
||||
(cond ((null paths) fail)
|
||||
((funcall goal? (path-state (first paths)))
|
||||
(values (first paths) paths))
|
||||
(t (let* ((path (pop paths))
|
||||
(state (path-state path)))
|
||||
(setf old-paths (insert-path path old-paths))
|
||||
(dolist (state2 (funcall successors state))
|
||||
(let* ((cost (+ (path-cost-so-far path)
|
||||
(funcall cost-fn state2)))
|
||||
(cost2 (funcall cost-left-fn state2))
|
||||
(path2 (make-path :state state2 :previous path
|
||||
:cost-so-far cost
|
||||
:total-cost (+ cost cost2)))
|
||||
old)
|
||||
(cond ((setf old (find-path state2 paths state=))
|
||||
(when (better-path path2 old)
|
||||
(setf paths (insert-path path2 (delete old paths)))))
|
||||
((setf old (find-path state2 old-paths state=))
|
||||
(when (better-path path2 old)
|
||||
(setf paths (insert-path path2 paths))
|
||||
(setf old-paths (delete old old-paths))))
|
||||
(t (setf paths (insert-path path2 paths))))))
|
||||
(a*-search paths goal? successors cost-fn cost-left-fn
|
||||
state= old-paths)))))
|
||||
(defun hash-a*-search (paths goal? successors cost-fn cost-left-fn
|
||||
&optional (state= #'eql) old-paths)
|
||||
"Find a path whose state satisfies goal?. Start with paths, and expand
|
||||
successors, exploring least cost first. When there are duplicate states,
|
||||
which are logged in a hash-table, keep the one with the lower cost
|
||||
and discard the other."
|
||||
(dbg 'search 0 ";; Search: ~a" paths)
|
||||
(cond ((null paths) fail)
|
||||
((funcall goal? (path-state (first paths)))
|
||||
(values (first paths) paths))
|
||||
(t (let* ((path (pop paths))
|
||||
(state (path-state path)))
|
||||
(setf old-paths (insert-path path old-paths))
|
||||
(dolist (state2 (funcall successors state))
|
||||
(let* ((cost (+ (path-cost-so-far path)
|
||||
(funcall cost-fn state2)))
|
||||
(cost2 (funcall cost-left-fn state2))
|
||||
(path2 (make-path :state state2 :previous path
|
||||
:cost-so-far cost
|
||||
:total-cost (+ cost cost2)))
|
||||
old)
|
||||
(cond ((setf old (find-path state2 paths state=))
|
||||
(when (better-path path2 old)
|
||||
(setf paths (insert-path paths (delete old paths)))))
|
||||
((setf old (find-path state2 old-paths state=))
|
||||
(when (better-path path2 old)
|
||||
(setf paths (insert-path path2 paths))
|
||||
(setf old-paths (delete old old-paths))))
|
||||
(t (setf paths (insert-path path2 paths))))))
|
||||
(hash-a*-search paths goal? successors cost-fn cost-left-fn
|
||||
state= old-paths)))))
|
||||
|
||||
(defun applicable-ops (state)
|
||||
"Return a list of all ops that are applicable now."
|
||||
(remove-if-not (lambda (op) (subsetp (op-preconds op) state :test #'equal))
|
||||
*ops*))
|
||||
(defun gps-successors (state)
|
||||
"Return a list of states reachable from this one using ops."
|
||||
(mapcar (lambda (op)
|
||||
(append (remove-if (lambda (x)
|
||||
(member x (op-del-list op) :test #'equal))
|
||||
state)
|
||||
(op-add-list op)))
|
||||
(applicable-ops state)))
|
||||
(defun search-gps (start goal &optional (beam-width 10))
|
||||
"Search for a sequence of operators leading to goal."
|
||||
(remove-if-not
|
||||
#'action?
|
||||
(beam-search (cons '(start) start)
|
||||
(lambda (state) (subsetp goal state :test #'equal))
|
||||
#'gps-successors
|
||||
(lambda (state)
|
||||
(+ (cound-if #'action? state)
|
||||
(count-if (lambda (con)
|
||||
(not (member con state :test #'equal))))))
|
||||
beam-width)))
|
||||
|
||||
(defun compose (&rest functions)
|
||||
"Return the function that except one argument and nestedly call functions."
|
||||
(lambda (x) (reduce #'funcall functions :from-end t :initial-value x)))
|
||||
Loading…
Reference in New Issue