 		
 ;  TODO: add timeout
 ;; TODO: add preempt
-
 ;; TODO: attach bottles
 ;; TODO: ignore result of arm action.
 ;  TODO: integrate with find_bottles
-;  TODO: extend scrips.
+
+;;  TODO: extend scrips.
     ; Smart script for grasping - move base + ask for new point, if needed.
-
+;; TODO: fix rotate-toa-angle
 
 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;     States      ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;



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

(defn close-gripper 
  ([nh] (close-gripper nh 45))
  ([nh force] 
     (apply-gripper-force nh true (- force))
     (attach-bottle nh)
     ))



	  (move-arm-to-pos ~'nh ~'j ~'upright?)))
             
     (defn ~'open [] (open-gripper ~'nh))
     (defn ~'close ([] (~'close 45)) ([~'f] (close-gripper ~'nh ~'f false)))

     (defn ~'home [] (~'go-arm-plan "home"))
     
