 		    (if right? *rarm-joints* *larm-joints*)
 		    (safe-get* (if right? *rarm-joint-states* *larm-joint-states*) name))))))
 
+(defn arm-l2-distance [j1 j2]
+  (Math/sqrt (double (apply + (map #(* % %) 
+    (for [[v1 v2 roll?] (map vector j1 j2 *arm-joint-wraps*)]
+      (let [diff (Math/abs (double (- v1 v2)))]
+	(if roll? (if (> diff Math/PI) (- (* 2 Math/PI) diff) diff) diff))))))))
+
+
+(defn interpolate-arm-joint [v1 v2 w wrap?]
+  (let [dist (double (- v2 v1))]
+    (if (or (not wrap?) (< (Math/abs dist) Math/PI))
+        (+ v1 (* dist w))
+      (norm-angle (- v1 (* (- (* 2 Math/PI) dist) w))))))
+
+(defn interpolate-arm-joints [j1 j2 w]
+  (map (fn [v1 v2 wrap?] (interpolate-arm-joint v1 v2 w wrap?)) j1 j2 *arm-joint-wraps*))
 
 ;;; Using known states ;;;
 

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; Base ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;


(defstruct robot-base-state    :class :x :y :theta)


(def *arm-joints*
  ["shoulder_pan_joint" "shoulder_lift_joint" "upper_arm_roll_joint" 
   "elbow_flex_joint" "forearm_roll_joint" "wrist_flex_joint" "wrist_roll_joint"])
(def *arm-joint-wraps* [false false false false true false true])

(def *larm-joints* (map #(str "l_" %) *arm-joints*))
(def *rarm-joints* (map #(str "r_" %) *arm-joints*))
