 (defn move-arm-directly-to-state [#^NodeHandle nh arm-state]
   (let [right? (isa? (:type arm-state) ::Right)
 	{:keys [jointnames jointpositions]} (get-current-arm-state-msg nh right?)]
-    (call-srv nh (str "/" (if right? "r" "l") "_arm_joint_trajectory_controller/TrajectoryStart")
+    (call-srv-cached nh (str "/" (if right? "r" "l") "_arm_joint_trajectory_controller/TrajectoryStart")
       (map-msg
        {:class TrajectoryStart$Request :hastiming 0 :requesttiming 0
 	:traj {:class JointTraj

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

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; Get, move to state ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

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

