     (.shutdown pub)))
 
 
-(defn move-base-forward-unsafe 
-  "Directly moves using base controllers, without invoking planning"
-  [#^NodeHandle nh distance]
+(defn move-base-rel
+  "Directly moves using base controllers (unsafe), without invoking planning"
+  [#^NodeHandle nh coord distance]
+  (assert (#{:vx :vy} coord))
+  (let [distance (double distance)
+	dir (Math/signum distance)
+	distance (Math/abs distance)]
    (move-base-unsafe nh
     (fn [init-pose current-pose]
       (let [dist (- distance (point-distance (:position init-pose) (:position current-pose)))]
-	{:vel {:vx (if (> dist 0.1) 0.5 (* dist 3))}}))
+	{:vel {coord (* dir (if (> dist 0.1) 0.5 (* dist 3)))}}))
     (fn [init-pose current-pose]
       (let [dist (- distance (point-distance (:position init-pose) (:position current-pose)))]
-	(< dist 0.005)))))
+	(< (Math/abs (double dist)) 0.005))))))
 
 (defn norm-angle [a]
   (cond (> a Math/PI) (- a (* 2 Math/PI))
 	(< a (- Math/PI)) (+ a (* 2 Math/PI))
 	:else a))
 
-(defn spin-base-unsafe 
+(defn spin-base
   "Spins base at a specified vecocity (pos = ccw) for a specified time."
   [#^NodeHandle nh vel secs]
   (let [pub (.advertise nh "/cmd_vel" (PoseDot.) 1)

	(recur)))
    (.shutdown pub)))

(defn spin-base-to-angle-unsafe
  "Spins base to a desired angle, with no collision checking."
  [#^NodeHandle nh angle]
  (move-base-unsafe nh 

     :points (map (fn [m t] {:positions (map m ks) :time t}) [init-joints final-joints] [0 0.2])}))




 ;; TODO: handle wraparound
(defn make-smooth-joint-trajectory [init-joints final-joints step-l2 step-time]
