(ns org.aurellem.self-organizing-touch

   "Using free play to automatically organize touch perception into regions."

   {:author "Robert McIntyre"}

   (:use (cortex world util import body sense

                 hearing touch vision proprioception movement

                 test))

   (:use [clojure set pprint])

   (:import (com.jme3.math ColorRGBA Vector3f))

   (:import java.io.File)

   (:import com.jme3.audio.AudioNode)

   (:import com.aurellem.capture.RatchetTimer)

   (:import (com.aurellem.capture Capture IsoTimer))

   (:import (com.jme3.math Vector3f ColorRGBA)))

 

 (use 'org.aurellem.worm-learn)

 (dorun (cortex.import/mega-import-jme3))

 

 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

 ;; A demonstration of self organiging touch maps through experience. ;

 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

 

 (def single-worm-segment-view

   [(Vector3f. 2.0681207, -6.1406755, 1.6106138)

    (Quaternion. -0.15558705, 0.843615, -0.3428654, -0.38281822)])

 

 (def worm-single-segment-muscle-labels

   [:lift-1 :lift-2 :roll-1 :roll-2])

 

 (defn touch-kinesthetics []

   [[170 :lift-1 40]

    [190 :lift-1 19]

    [206 :lift-1  0]

 

    [400 :lift-2 40]

    [410 :lift-2  0]

 

    [570 :lift-2 40]

    [590 :lift-2 21]

    [606 :lift-2  0]

 

    [800 :lift-1 30]

    [809 :lift-1 0]

 

    [900 :roll-2 40]

    [905 :roll-2 20]

    [910 :roll-2  0]

 

    [1000 :roll-2 40]

    [1005 :roll-2 20]

    [1010 :roll-2  0]

    

    [1100 :roll-2 40]

    [1105 :roll-2 20]

    [1110 :roll-2  0]

    ])

 

 (defn single-worm-segment []

   (load-blender-model "Models/worm/worm-single-segment.blend"))

 

 (defn worm-segment []

   (let [model (single-worm-segment)]

     {:body (doto model (body!))

      :touch (touch! model)

      :proprioception (proprioception! model)

      :muscles (movement! model)}))

 

 

 (defn worm-segment-defaults []

   (let [direct-control (worm-direct-control worm-muscle-labels 40)]

     (merge (worm-world-defaults)

            {:worm worm-segment

             :view single-worm-segment-view

             :experience-watch nil

             :motor-control

             (motor-control-program

              worm-single-segment-muscle-labels

              (touch-kinesthetics))

             :end-frame 1200})))

 

 (def full-contact [(float 0.0) (float 0.1)])

 

 (defn pure-touch?

   "This is worm specific code to determine if a large region of touch

    sensors is either all on or all off."

   [[coords touch :as touch-data]]

   (= (set (map first touch)) (set full-contact)))

 

 (defn remove-similar

   [coll]

   (loop [result () coll (sort-by (comp - count) coll)]

     (if (empty? coll) result

         (let  [[x & xs] coll

                c (count x)]

           (if (some

                (fn [other-set]

                  (let [oc (count other-set)]

                    (< (- (count (union other-set x)) c) (* oc 0.1))))

                xs)

             (recur result xs)

             (recur (cons x result) xs))))))

 

 (def all-touch-coordinates

   (concat

    (rect-region [0  15] [7  22])

    (rect-region [8   0] [14 29])

    (rect-region [15 15] [22 22])))

 

 (defn view-touch-region

   ([coords out]

      (let [touched-region

            (reduce

             (fn [m k]

               (assoc m k [0.0 0.1]))

             (zipmap all-touch-coordinates (repeat [0.1 0.1])) coords)

            data

            [[(vec (keys touched-region)) (vec (vals touched-region))]]

            touch-display (view-touch)]

        (touch-display data out)))

   ([coords] (view-touch-region nil)))

 

 

 (defn learn-touch-regions []

   (let [experiences (atom [])

         world (apply-map

                worm-world

                (assoc (worm-segment-defaults)

                  :experiences experiences

                  :record (File. "/home/r/proj/cortex/thesis/video/touch-learn-2/")))]

     (run-world world)

     (->>

      @experiences

      (drop 175)

      ;; access the single segment's touch data

      (map (comp first :touch))

      ;; only deal with "pure" touch data to determine surfaces

      (filter pure-touch?)

      ;; associate coordinates with touch values

      (map (partial apply zipmap))

      ;; select those regions where contact is being made

      (map (partial group-by second))

      (map #(get % full-contact))

      (map (partial map first))

      ;; remove redundant/subset regions

      (map set)

      remove-similar)))

 

 

 (def all-touch-coordinates

   (concat

    (rect-region [0  15] [7  22])

    (rect-region [8   0] [14 29])

    (rect-region [15 15] [22 22])))

 

 (defn view-touch-region [coords]

   (let [touched-region

         (reduce

          (fn [m k]

            (assoc m k [0.0 0.1]))

          (zipmap all-touch-coordinates (repeat [0.1 0.1])) coords)

         data

         [[(vec (keys touched-region)) (vec (vals touched-region))]]

         touch-display (view-touch)]

     (dorun (repeatedly 5 #(touch-display data)))))

 

 (defn learn-and-view-touch-regions []

   (map view-touch-region

        (learn-touch-regions)))