From 6c0ea3cc3364733e8d8fe7625b128f817d55b2cd Mon Sep 17 00:00:00 2001 From: Nguyễn Gia Phong Date: Thu, 4 Jul 2019 22:11:37 +0700 Subject: [PAIP] Upload GPS and move on --- paip/gps.lisp | 559 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 559 insertions(+) create mode 100644 paip/gps.lisp diff --git a/paip/gps.lisp b/paip/gps.lisp new file mode 100644 index 0000000..8501bc1 --- /dev/null +++ b/paip/gps.lisp @@ -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-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-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))) -- cgit 1.4.1