     (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)]

  )

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

      (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)))
