     (move-base-rel nh :vx (- objx gx 0.15) 1.0 #_0.5)
     true
     ))
+
+(defn point-head [nh bl-point] 
+  (put-single-message nh "/head_controller/point_head" 
+    (map-msg PointStamped {:header {:frame_id "/base_link" :stamp (.now nh)} 
+			   :point (make-point bl-point)}) 1))
   
 (defn pt [x] (println x) x)
 
 (defn grasp-object "obj in base-link" [nh obj]
   (and 
+   (do (point-head nh obj) true)
    (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))))
+   (pt (move-arm-to-state nh (arm-joint-state true "home") true 60.0))
+   (do (point-head nh [1 0 1.2]) true)))
 
 (defn grasp-rviz [nh]
   (let [[obj-map obj-bl] (get-rviz-points nh true)]

      (do (future-call laser-fast)
	  (let [bottles (map #(decode-point (:point %)) bottles)]
	    (println "Got bottles" bottles)
	    (util/first-maximal-element 
	     #(- 100 (Math/abs (double (second %))))
	     bottles)
	    )))))

(defn compute-base-grasp-rect 

	  (move-arm-to-pos ~'nh ~'j ~'upright? 30.0)))
             
     (defn ~'open [] (open-gripper ~'nh))
     (defn ~'close ([] (~'close 60)) ([~'f] (close-gripper ~'nh ~'f false)))
     (defn ~'throw [] (do-throw ~'nh "_new"))

     (defn ~'homeu [] (~'go-arm-plan "home"))
     (defn ~'home [] (~'go-arm-plan "home" true))
;     (defn ~'homeu [] (~'go-arm "home"))
     
     (defn ~'face-bar [] (spin-base-to-bar ~'nh))
     (defn ~'face-window [] (spin-base-from-bar ~'nh))
