 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; Get, move to state ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
 
 (defn get-current-arm-state-msg [nh right?]
-  (call-srv nh (str "/" (if right? "r" "l") 
+  (call-srv-cached nh (str "/" (if right? "r" "l") 
 		    "_arm_joint_trajectory_controller/TrajectoryQuery" )
 	    (map-msg {:class TrajectoryQuery$Request :trajectoryid -1})))
 

    (cons (> (:in_collision res) 0)
	  (lazy-seq [(apply hash-map (interleave (:link_names res) (:link_poses res)))]))))

(defn robot-forward-kinematics
  "Like forward-kinematics, but takes a robot state and possibly world state,
   if a collision map is to be published."
 ([#^NodeHandle nh robot]
    (forward-kinematics nh (get-joint-map robot)))
 ([#^NodeHandle nh robot world]
    (put-single-message nh "/fk_node/collision_map" 
      (map-msg (world->collision-map world)) 1)
    (robot-forward-kinematics nh robot)))

(defn rand-double [[mn mx]]

   pose constraints are lists of PoseConstraints maps -- no shortcuts for now.
   Init-joints should include base and torso, in general."
  [#^NodeHandle nh right? world robot-state joint-constraints pose-constraints]
  (put-single-message-cached nh "collision_map_future" (map-msg (world->collision-map world)))
;  (println "\n\n\n\n\n\n\n\n" right?)
;  (println (doall (map make-kinematic-joint (get-joint-map robot-state))))
;  (println "\n\n\n")
;  (println pose-constraints)
  (call-srv-cached nh "/plan_kinematic_path"
   (map-msg 
     {:class MotionPlan$Request :times 1 :allowed_time 0.5
      :params (if right? *rarm-params* *larm-params*)
