   [robot x y theta]
   (call-service robot :motion "moveTo" [x y theta]))
 
-(defn get-joint-angles
+(defn- get-joint-angles-internal
   "Return a map of the current joint angles for a robot"
-  [robot]
-  (-> (call-service robot :motion "getAngles"
-                    ["Body" Boolean/TRUE])
+  [robot useSensors]
+  (->> (call-service robot :motion "getAngles" ["Body" useSensors])
       (.get)
       (zipmap joint-names)))
+
+(defn get-joint-angles
+  ([robot] (get-joint-angles-internal robot Boolean/TRUE))
+  ([robot useSensors] (get-joint-angles-internal useSensors)))
 
 (defn get-body-names
   "Return names of joints in the specified chain"

  (->
   (call-service robot :motion "getBodyNames"  [name])
   (.get)))

;; broken - can't construct variant holding array of values
(defn set-joint-angles

  [actions]
  (->> actions
       (map :joints)
       (filter identity)
       (apply merge)))

(defn- clip
  [value min max]
  (cond
   (> value max) max
   (< value min) min
   :else value))

(defn- compute-joint-speed
  "Compute fraction of max speed for joint movement in radians"
  [execution-time-seconds cur-pos-rad desired-pos-rad limits]
  (let [max-change (limits 2)
        desired-change (Math/abs (- cur-pos-rad desired-pos-rad))
        speed (/ desired-change (* max-change execution-time-seconds))]
    (clip speed 0.01 1.0)))

(defn- motion-task
  [robot joint-name joint-pos-deg execution-time-seconds cur-joints limits]
  (let [pos-rad (Math/toRadians joint-pos-deg)
        speed-fraction (compute-joint-speed execution-time-seconds
                                            (cur-joints joint-name)
                                            pos-rad
                                            (limits joint-name))]
    (call-service robot :motion "angleInterpolationWithSpeed"
                  [joint-name
                   (into-array Object [(Float. pos-rad)])
                   (Float. speed-fraction)])))

(defn- do-joints
  "Takes a map of joint changes and a duration and builds a motion task for each joint with the appropriate speed"
  [robot duration joint-changes]
  (if (seq joint-changes)
    (let [cur-joints (get-joint-angles-internal robot Boolean/FALSE)
          limits (load-joint-limits robot)]
      (map (fn [[j v]] (motion-task robot j v duration cur-joints limits))
           joint-changes))))

;; TODO make the duration and channel params optional
(defn donao
  "Accepts a number of parameters specifying robot actions and executes them"
  [robot duration chan & actions]
  (let [joint-changes (only-joint-actions actions)
        joint-tasks (do-joints robot duration joint-changes)]
    ))
