 ;	    (if-let [p (get *robot-joint-limits* j)]
 ;	        (vec (map read-string p))
 ;	      [(- Math/PI) Math/PI])])))
-;(defn rand-joint-config [right?]
-;  (into {}
-;    (map (fn [[k v]] [k (rand-double v)]) (if right? *rarm-joint-limits* *larm-joint-limits*))))
+
 
 
 ;(defn move-arm-directly-to-pose [nh right? pose-stamped ]

      (map-msg (world->collision-map world)) 1)
    (robot-forward-kinematics nh robot)))

(defn safe-inverse-kinematics 
  ([#^NodeHandle nh robot world tries]
     (put-single-message "/fk_node/collision_map" 
       (map-msg (world->collision-map world)) 1)
     (loop [tries tries]
       (when (> tries 0)
	 
     


; (inverse-kinematics nh true {:class PoseStamped :header *tll-header* :pose {:class Pose :position {:class Point :x 0.53 :y -0.02 :z -0.38} :orientation {:class Quaternion :x 0.0 :y 0.0 :z 0.0 :w 1.0}}} (:joint-angle-map (get-current-arm-state nh true)))

;(defn rand-double [[mn mx]]
;  (+ mn (rand (- mx mn))))
;
;  (def *rarm-joint-limits*
;       (into {}

			:joints (map make-kinematic-joint joint-map)}))]
;     (println res)
     (assert (= (count (:link_names res)) (count (:link_poses res))))
    (cons (> (:in_collision res) 0)
	  (lazy-seq [(apply hash-map (interleave (:link_names res) (:link_poses res)))]))))

(defn robot-forward-kinematics
