     (fn [init-pose current-pose]
       (let [ac (quaternion->angle (:orientation current-pose))
 	    norm-diff (double (norm-angle (- angle ac)))]
-	(< (Math/abs norm-diff) 0.01))))))
+	(< (Math/abs norm-diff) 0.03))))))
 
 (defn spin-base-rel
   "Spins base by a desired angle, with no collision checking."

    (move-base-rel nh :vx (- objx gx 0.15) 1.0 #_0.5)
    true
    ))
  
(defn pt [x] (println x) x)

(defn grasp-object "obj in base-link" [nh obj]
  (and 
   (do (pt (arm-to-grasp nh obj)) true)
   (do (pt (open-gripper nh)) true)
   (do (Thread/sleep 3000) (final-approach nh obj) true)
   (do (pt (close-gripper nh)) true)
   (do (Thread/sleep 3000) (move-base-rel nh :vx -0.3) true)
   (pt (move-arm-to-state nh (arm-joint-state true "home") true 60.0))))

(defn grasp-rviz [nh]
  (let [[obj-map obj-bl] (get-rviz-points nh true)]

      (let [dist (- distance (point-distance (:position init-pose) (:position current-pose)))]
;	(println dist)
;	(println "commanding" (* dir (if (> dist 0.1) 0.2 (* dist 3))))
	{:vel {coord (* dir (cond (> dist 0.1) (* speed 0.2) 
				  (< dist -0.1) 0
				  :else (* (max dist 0) speed 3)))}}))
    (fn [init-pose current-pose]
      (let [dist (- distance (point-distance (:position init-pose) (:position current-pose)))]
	(or (< (Math/abs (double dist)) 0.005)
	    (< dist -0.09))))))))

(defn norm-angle [a]
  (cond (> a (+ Math/PI 0.0000001)) (recur (- a (* 2 Math/PI)))
