    (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.2) 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))))
 
 (defn grasp-rviz [nh]
   (let [[obj-map obj-bl] (get-rviz-points nh true)]
     (grasp-object nh obj-bl)))
+
+(defn grasp-nearest [nh ht]
+  (grasp-object nh (wait-for-bottle nh ht)))
 
 
 

(defn find-bottles [nh z]
  (:pts (call-srv nh "/find_bottles" (map-msg FindBottles$Request {:z z}))))

(defn compute-base-grasp-rect 
  "Given a rectangular table and point in map frame, compute a good base state along x edge.
   Return nil if no pose is thought feasible"



(defn open-gripper [nh]
  (apply-gripper-force nh true 100)
  (unattach-bottle nh)
  )

(defn close-gripper 
  ([nh] (close-gripper nh 45 false))
  ([nh force empty?] 
     (apply-gripper-force nh true (- force))
     (when-not empty? (attach-bottle nh))
     ))


