             (for [dir dirs]
               [(make-base-dir s dir) (make-gripper-dir s dir)
                (make-pickup-dir s dir) (make-putdown-dir s dir)]))))
-     (into {[:parked?] true [:gxo] 0 [:gyo] 0} 
-           (for [[o _ goal] objects :when goal] [[:at-goal? o] true])))))
+     (into {[:parked?] true [:gripper-offset] [0 0]} 
+           (for [[o _ goal] objects :when goal] [[:at-goal? o] true]))))))
+
+; (print-state (initial-state (make-2d-manipulation-env [10 10] [1 1] [ [ [4 4] [6 6] ] ] [ [:a [5 5] #{ [4 4] } ] ] 2)))
+; (uniform-cost-search (make-2d-manipulation-env [10 10] [1 1] [ [ [4 4] [6 6] ] ] [ [:a [5 5] #{ [4 4] } ] ] 2))
+
+(comment 
+ (defn make-2d-manipulation-env [size start obstacles objects g-rad]
+   (let [[width height] size
+         [bx by]        start
+         border-regions [[[0 0] [0 height]] [[width 0] [width height]]
+                         [[0 0] [width 0]] [[0 height] [width height]]]
+         border-cells   (set (mapcat region-cells border-regions))
+         obstacle-cells (set (mapcat region-cells obstacles))
+         all-cells      (set (region-cells [[0 0] size]))
+         freespace      (clojure.set/difference all-cells border-cells obstacle-cells)
+         legal-go       (set (filter (fn [[x y]] (<= (+ (util/abs x) (util/abs y)) g-rad))
+                                     (region-cells [[(- g-rad) (- g-rad)] [g-rad g-rad]])))]
+     (env/SimpleFactoredEnv
+      (into {:const (into {[:size] size [:freespace] freespace [:legal-go] legal-go [:objects] objects
+                           [:map] (reduce (fn [m [x y]] (assoc-in m [y x] \.))
+                                          (reduce (fn [m [x y]] (assoc-in m [y x] \*))
+                                                  (vec (repeat (inc height) (vec (repeat (inc width) \ ))))
+                                                  border-cells)
+                                          obstacle-cells)}
+                          (for [[o _ goal] objects :when goal] [[:goal o] goal]))
+             [:bx] bx [:by] by [:gxo] 0 [:gyo] 0 [:holding] nil [:parked?] true}
+            (apply concat
+                   (for [[x y] all-cells]     [[:object-at x y] nil])
+                   (for [[x y] border-cells] [[:object-at x y] :border])
+                   (for [[o [ox oy] goal] objects]
+                     [[[:x o] ox] [[:y o] oy] 
+                      [[:at-goal? o] (if goal false true)] 
+                      [[:object-at ox oy] o]])))
+      (fn [s]
+        (filter #(and % (env/applicable? % s))
+                (apply concat
+                       [(make-park s) (make-unpark s)]
+                       (for [dir dirs]
+                         [(make-base-dir s dir) (make-gripper-dir s dir)
+                          (make-pickup-dir s dir) (make-putdown-dir s dir)]))))
+      (into {[:parked?] true [:gxo] 0 [:gyo] 0} 
+            (for [[o _ goal] objects :when goal] [[:at-goal? o] true]))))))
 
 (defn state-map [s]
   (let [const   (env/get-var s :const)
-        bx      (env/get-var s [:bx])
-        by      (env/get-var s [:by])
-        gxo     (env/get-var s [:gxo])
-        gyo     (env/get-var s [:gyo])]
+        base    (env/get-var s [:base])
+        go      (env/get-var s [:gripper-offset])]
     (reduce (partial apply assoc-in)
             (get const [:map])
             (concat 
-             [[[(+ bx gxo) (+ by gyo)] \G]
-              [[bx by]                 \B]]
+             [[(add-pos base go) \G]
+              [base              \B]]
              (for [[o] (get const [:objects])]
-               [[(env/get-var s [:x o]) (env/get-var s [:y o])] (first (name o))])))))
+               [(env/get-var s [:pos o]) (first (name o))])))))
 
 (defn state-str [s]
   (apply str (util/str-join "\n" (map (partial apply str) (state-map s))) "\n"

(defn print-state [s] (print (state-str s)))


; (print-state (initial-state (make-2d-manipulation-env [10 10] [1 1] [ [ [4 4] [6 6] ] ] [ [:a [5 5] [ [4 4] [4 4] ] ] ] 2))) 
  (env/FactoredPrimitive [:park] {[:parked?] false} {[:parked?] true} -5))

(defn make-unpark [s]
  (env/FactoredPrimitive [:unpark] {[:parked?] true [:gripper-offset] [0 0]} {[:parked?] false} -5))

(defn make-base-dir [s [dirname dir]]
  (let [const (env/get-var s :const)
        base  (env/get-var s [:base])
        nbase (add-pos base dir)]
    (when (contains? (get const [:freespace]) nbase)
      (env/FactoredPrimitive 
       [:base dirname]
       {[:base] base [:parked?] false}
       {[:base] nbase}
       -2))))

(defn make-gripper-dir [s [dirname dir]]
  (let [const (env/get-var s :const)
        base  (env/get-var s [:base])
        go    (env/get-var s [:gripper-offest])
        ngo   (add-pos go dir)
        npos  (add-pos ngo base)]
    (when (contains? (get const [:legal-go]) ngo)
      (env/FactoredPrimitive 
       [:gripper dirname]
       {[:base] base [:gripper-offset] go [:parked?] true 
        [:object-at npos] nil}
       {[:gripper-offest] ngo}
       -1))))

(defn make-pickup [dirname base go o opos]
  (env/FactoredPrimitive 
   [:pickup dirname]
   {[:base] base [:gripper-offset] go [:parked?] true 
    [:pos o] opos [:holding] nil [:at-goal? o] false}
   {[:pos o] nil [:holding] o [:object-at opos] nil}
   -1))

(defn make-pickup-dir [s [dirname dir]]
  (let [base  (env/get-var s [:base])
        go    (env/get-var s [:gripper-offset])
        opos  (add-pos base go dir)
        o     (env/get-var s [:object-at opos])]
    (when (and o (not (= o :border)))
      (make-pickup dirname base go o opos))))


(defn make-putdown-dir [s [dirname dir]]
  (let [const (env/get-var s :const)
        o     (env/get-var s [:holding])
        base  (env/get-var s [:base])
        go    (env/get-var s [:gripper-offset])
        opos  (add-pos base go dir)]
    (when (and o (contains? (get const [:goal o]) opos))
      (env/FactoredPrimitive 
       [:putdown dirname]
       {[:base] base [:gripper-offset] go [:parked?] true 
        [:holding] o [:object-at opos] nil}
       {[:pos o] opos [:holding] nil [:object-at opos] o [:at-goal? o] true}
       -1))))


; obstacles are [[minx miny] [maxx maxy]]
; goals are sets os positions.
; objects are [name [cx cy] goal-pos-set] -- no goal means static 
; g-rad is gripper "radius" -- diamond shaped.

(defn make-2d-manipulation-env 
  ([size base obstacles objects g-rad]
     (make-2d-manipulation-env size base obstacles objects g-rad Integer/MAX_VALUE))
  ([size base obstacles objects g-rad n-base-samples]
     (make-2d-manipulation-env size base obstacles objects g-rad n-base-samples (make-random 1)))  
  ([size base obstacles objects g-rad n-base-samples #^Random random]
  (let [[width height] size
        border-regions [[[0 0] [0 height]] [[width 0] [width height]]
                        [[0 0] [width 0]] [[0 height] [width height]]]
        border-cells   (set (mapcat region-cells border-regions))
        obstacle-cells (set (mapcat region-cells obstacles))
        all-cells      (set (region-cells [[0 0] size]))
        freespace      (clojure.set/difference all-cells border-cells obstacle-cells)
        legal-go       (set (spiral-to g-rad))]
    (env/SimpleFactoredEnv
     (into {:const (into {[:size] size [:freespace] freespace [:legal-go] legal-go [:objects] objects
                          [:base-offsets] (pseudo-shuffle (spiral-to (inc g-rad)) random)
                          [:n-base-samples] n-base-samples
                          [:map] (reduce (fn [m [x y]] (assoc-in m [y x] \.))
                                   (reduce (fn [m [x y]] (assoc-in m [y x] \*))
                                      (vec (repeat (inc height) (vec (repeat (inc width) \ ))))
                                      border-cells)
                                   obstacle-cells)}
                         (for [[o _ goal] objects :when goal] [[:goal o] (set goal)]))
            [:base] base [:gripper-offset] [0 0] [:holding] nil [:parked?] true}
           (apply concat
            (for [cell all-cells]    [[:object-at cell] nil])
            (for [cell border-cells] [[:object-at cell] :border])
            (for [[o opos goal] objects]
              [[[:pos o] opos] 
               [[:at-goal? o] (if goal false true)] 
               [[:object-at opos] o]])))
     (fn [s]
       (filter #(and % (env/applicable? % s))
          (apply concat
