[PAIP] Upload GPS and move on

1 files changed, 559 insertions, 0 deletions

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