rlm@394: (ns org.aurellem.worm-learn rlm@394: "General worm creation framework." rlm@394: {:author "Robert McIntyre"} rlm@394: (:use (cortex world util import body sense rlm@408: hearing touch vision proprioception movement rlm@408: test)) rlm@394: (:import (com.jme3.math ColorRGBA Vector3f)) rlm@394: (:import java.io.File) rlm@394: (:import com.jme3.audio.AudioNode) rlm@397: (:import com.aurellem.capture.RatchetTimer) rlm@397: (:import (com.aurellem.capture Capture IsoTimer)) rlm@397: (:import (com.jme3.math Vector3f ColorRGBA))) rlm@406: rlm@413: (import org.apache.commons.math3.transform.TransformType) rlm@413: (import org.apache.commons.math3.transform.FastFourierTransformer) rlm@413: (import org.apache.commons.math3.transform.DftNormalization) rlm@413: rlm@406: (use 'clojure.pprint) rlm@408: (use 'clojure.set) rlm@394: (dorun (cortex.import/mega-import-jme3)) rlm@394: (rlm.rlm-commands/help) rlm@394: rlm@400: (load-bullet) rlm@394: rlm@399: (def hand "Models/test-creature/hand.blend") rlm@394: rlm@399: (defn worm-model [] rlm@399: (load-blender-model "Models/worm/worm.blend")) rlm@394: rlm@449: (defn worm [] rlm@449: (let [model (load-blender-model "Models/worm/worm.blend")] rlm@449: {:body (doto model (body!)) rlm@449: :touch (touch! model) rlm@449: :proprioception (proprioception! model) rlm@449: :muscles (movement! model)})) rlm@449: rlm@451: (defn worm* [] rlm@451: (let [model (load-blender-model "Models/worm/worm-of-the-imagination.blend")] rlm@451: {:body (doto model (body!)) rlm@451: :touch (touch! model) rlm@451: :proprioception (proprioception! model) rlm@451: :muscles (movement! model)})) rlm@451: rlm@451: rlm@400: (def output-base (File. "/home/r/proj/cortex/render/worm-learn/curl")) rlm@394: rlm@397: rlm@399: (defn motor-control-program rlm@399: "Create a function which will execute the motor script" rlm@406: [muscle-labels rlm@399: script] rlm@399: (let [current-frame (atom -1) rlm@399: keyed-script (group-by first script) rlm@399: current-forces (atom {}) ] rlm@399: (fn [effectors] rlm@399: (let [indexed-effectors (vec effectors)] rlm@399: (dorun rlm@399: (for [[_ part force] (keyed-script (swap! current-frame inc))] rlm@399: (swap! current-forces (fn [m] (assoc m part force))))) rlm@399: (doall (map (fn [effector power] rlm@399: (effector (int power))) rlm@399: effectors rlm@406: (map #(@current-forces % 0) muscle-labels))))))) rlm@397: rlm@404: (defn worm-direct-control rlm@404: "Create keybindings and a muscle control program that will enable rlm@404: the user to control the worm via the keyboard." rlm@404: [muscle-labels activation-strength] rlm@404: (let [strengths (mapv (fn [_] (atom 0)) muscle-labels) rlm@404: activator rlm@404: (fn [n] rlm@404: (fn [world pressed?] rlm@404: (let [strength (if pressed? activation-strength 0)] rlm@404: (swap! (nth strengths n) (constantly strength))))) rlm@404: activators rlm@404: (map activator (range (count muscle-labels))) rlm@404: worm-keys rlm@404: ["key-f" "key-r" rlm@404: "key-g" "key-t" rlm@413: "key-h" "key-y" rlm@404: "key-j" "key-u" rlm@413: "key-k" "key-i" rlm@413: "key-l" "key-o"]] rlm@404: {:motor-control rlm@404: (fn [effectors] rlm@404: (doall rlm@404: (map (fn [strength effector] rlm@404: (effector (deref strength))) rlm@404: strengths effectors))) rlm@404: :keybindings rlm@404: ;; assume muscles are listed in pairs and map them to keys. rlm@404: (zipmap worm-keys activators)})) rlm@400: rlm@400: ;; These are scripts that direct the worm to move in two radically rlm@400: ;; different patterns -- a sinusoidal wiggling motion, and a curling rlm@400: ;; motions that causes the worm to form a circle. rlm@400: rlm@400: (def curl-script rlm@415: [[150 :d-flex 40] rlm@415: [250 :d-flex 0]]) rlm@400: rlm@400: (def period 18) rlm@400: rlm@404: (def worm-muscle-labels rlm@414: [:base-ex :base-flex rlm@414: :a-ex :a-flex rlm@414: :b-ex :b-flex rlm@414: :c-ex :c-flex rlm@414: :d-ex :d-flex]) rlm@399: rlm@399: (defn gen-wiggle [[flexor extensor :as muscle-pair] time-base] rlm@399: (let [period period rlm@399: power 45] rlm@399: [[time-base flexor power] rlm@399: [(+ time-base period) flexor 0] rlm@399: [(+ time-base period 1) extensor power] rlm@399: [(+ time-base (+ (* 2 period) 2)) extensor 0]])) rlm@399: rlm@399: (def wiggle-script rlm@414: (mapcat gen-wiggle (repeat 4000 [:a-ex :a-flex]) rlm@406: (range 100 1000000 (+ 3 (* period 2))))) rlm@399: rlm@399: rlm@415: (defn shift-script [shift script] rlm@415: (map (fn [[time label power]] [(+ time shift) label power]) rlm@415: script)) rlm@415: rlm@415: (def do-all-the-things rlm@415: (concat rlm@415: curl-script rlm@415: [[300 :d-ex 40] rlm@415: [320 :d-ex 0]] rlm@415: (shift-script 280 (take 16 wiggle-script)))) rlm@415: rlm@400: ;; Normally, we'd use unsupervised/supervised machine learning to pick rlm@400: ;; out the defining features of the different actions available to the rlm@400: ;; worm. For this project, I am going to explicitely define functions rlm@400: ;; that recognize curling and wiggling respectively. These functions rlm@400: ;; are defined using all the information available from an embodied rlm@400: ;; simulation of the action. Note how much easier they are to define rlm@400: ;; than if I only had vision to work with. Things like scale/position rlm@400: ;; invariance are complete non-issues here. This is the advantage of rlm@400: ;; body-centered action recognition and what I hope to show with this rlm@400: ;; thesis. rlm@400: rlm@405: rlm@415: ;; curled? relies on proprioception, resting? relies on touch, rlm@415: ;; wiggling? relies on a fourier analysis of muscle contraction, and rlm@415: ;; grand-circle? relies on touch and reuses curled? as a gaurd. rlm@405: rlm@405: (defn curled? rlm@405: "Is the worm curled up?" rlm@405: [experiences] rlm@405: (every? rlm@405: (fn [[_ _ bend]] rlm@405: (> (Math/sin bend) 0.64)) rlm@405: (:proprioception (peek experiences)))) rlm@405: rlm@411: (defn rect-region [[x0 y0] [x1 y1]] rlm@411: (vec rlm@411: (for [x (range x0 (inc x1)) rlm@411: y (range y0 (inc y1))] rlm@411: [x y]))) rlm@407: rlm@415: (def worm-segment-bottom (rect-region [8 15] [14 22])) rlm@407: rlm@411: (defn contact rlm@411: "Determine how much contact a particular worm segment has with rlm@411: other objects. Returns a value between 0 and 1, where 1 is full rlm@411: contact and 0 is no contact." rlm@415: [touch-region [coords contact :as touch]] rlm@411: (-> (zipmap coords contact) rlm@415: (select-keys touch-region) rlm@411: (vals) rlm@411: (#(map first %)) rlm@411: (average) rlm@411: (* 10) rlm@411: (- 1) rlm@411: (Math/abs))) rlm@406: rlm@415: (defn resting? rlm@443: "Is the worm resting on the ground?" rlm@415: [experiences] rlm@415: (every? rlm@415: (fn [touch-data] rlm@415: (< 0.9 (contact worm-segment-bottom touch-data))) rlm@415: (:touch (peek experiences)))) rlm@415: rlm@415: (defn vector:last-n [v n] rlm@415: (let [c (count v)] rlm@415: (if (< c n) v rlm@415: (subvec v (- c n) c)))) rlm@415: rlm@413: (defn fft [nums] rlm@414: (map rlm@414: #(.getReal %) rlm@414: (.transform rlm@414: (FastFourierTransformer. DftNormalization/STANDARD) rlm@414: (double-array nums) TransformType/FORWARD))) rlm@413: rlm@413: (def indexed (partial map-indexed vector)) rlm@413: rlm@414: (defn max-indexed [s] rlm@414: (first (sort-by (comp - second) (indexed s)))) rlm@414: rlm@400: (defn wiggling? rlm@405: "Is the worm wiggling?" rlm@405: [experiences] rlm@451: (let [analysis-interval 96] rlm@414: (when (> (count experiences) analysis-interval) rlm@414: (let [a-flex 3 rlm@414: a-ex 2 rlm@414: muscle-activity rlm@414: (map :muscle (vector:last-n experiences analysis-interval)) rlm@414: base-activity rlm@451: (map #(- (% a-flex) (% a-ex)) muscle-activity) rlm@451: accept? rlm@451: (fn [activity] rlm@451: (->> activity (fft) (take 20) (map #(Math/abs %)) rlm@451: (max-indexed) (first) (<= 2)))] rlm@451: (or (accept? (take 64 base-activity)) rlm@451: (accept? (take 64 (drop 20 base-activity)))))))) rlm@451: rlm@451: rlm@414: rlm@415: (def worm-segment-bottom-tip (rect-region [15 15] [22 22])) rlm@414: rlm@415: (def worm-segment-top-tip (rect-region [0 15] [7 22])) rlm@414: rlm@415: (defn grand-circle? rlm@415: "Does the worm form a majestic circle (one end touching the other)?" rlm@415: [experiences] rlm@420: (and (curled? experiences) rlm@415: (let [worm-touch (:touch (peek experiences)) rlm@415: tail-touch (worm-touch 0) rlm@415: head-touch (worm-touch 4)] rlm@451: (and (< 0.1 (contact worm-segment-bottom-tip tail-touch)) rlm@451: (< 0.1 (contact worm-segment-top-tip head-touch)))))) rlm@400: rlm@418: rlm@449: (declare phi-space phi-scan debug-experience) rlm@418: rlm@418: rlm@418: rlm@400: (def standard-world-view rlm@400: [(Vector3f. 4.207176, -3.7366982, 3.0816958) rlm@400: (Quaternion. 0.11118768, 0.87678415, 0.24434438, -0.3989771)]) rlm@400: rlm@400: (def worm-side-view rlm@400: [(Vector3f. 4.207176, -3.7366982, 3.0816958) rlm@400: (Quaternion. -0.11555642, 0.88188726, -0.2854942, -0.3569518)]) rlm@400: rlm@400: (def degenerate-worm-view rlm@400: [(Vector3f. -0.0708936, -8.570261, 2.6487997) rlm@400: (Quaternion. -2.318909E-4, 0.9985348, 0.053941682, 0.004291452)]) rlm@399: rlm@404: (defn worm-world-defaults [] rlm@404: (let [direct-control (worm-direct-control worm-muscle-labels 40)] rlm@430: (merge direct-control rlm@430: {:view worm-side-view rlm@430: :record nil rlm@430: :experiences (atom []) rlm@430: :experience-watch debug-experience rlm@451: :worm worm rlm@430: :end-frame nil}))) rlm@407: rlm@404: (defn dir! [file] rlm@410: (if-not (.exists file) rlm@404: (.mkdir file)) rlm@404: file) rlm@405: rlm@405: (defn record-experience! [experiences data] rlm@405: (swap! experiences #(conj % data))) rlm@405: rlm@444: (defn enable-shadows [world] rlm@444: (let [bsr (doto rlm@444: (BasicShadowRenderer. (asset-manager) 512) rlm@444: (.setDirection (.normalizeLocal (Vector3f. 1 -1 -1))))] rlm@444: (.addProcessor (.getViewPort world) bsr))) rlm@443: rlm@444: (defn enable-good-shadows [world] rlm@444: (let [pssm rlm@444: (doto (PssmShadowRenderer. (asset-manager) 1024 3) rlm@444: (.setDirection (.normalizeLocal (Vector3f. -1 -3 -1))) rlm@444: (.setLambda (float 0.55)) rlm@444: (.setShadowIntensity (float 0.6)) rlm@444: (.setCompareMode PssmShadowRenderer$CompareMode/Software) rlm@444: (.setFilterMode PssmShadowRenderer$FilterMode/Bilinear))] rlm@444: (.addProcessor (.getViewPort world) pssm))) rlm@444: rlm@449: (defn debug-experience rlm@449: [experiences text] rlm@449: (cond rlm@449: (grand-circle? experiences) (.setText text "Grand Circle") rlm@449: (curled? experiences) (.setText text "Curled") rlm@449: (wiggling? experiences) (.setText text "Wiggling") rlm@451: (resting? experiences) (.setText text "Resting") rlm@451: :else (.setText text "Unknown"))) rlm@443: rlm@445: rlm@399: (defn worm-world rlm@451: [& {:keys [record motor-control keybindings view experiences rlm@451: worm end-frame experience-watch] :as settings}] rlm@407: (let [{:keys [record motor-control keybindings view experiences rlm@451: worm end-frame experience-watch]} rlm@404: (merge (worm-world-defaults) settings) rlm@449: rlm@404: touch-display (view-touch) rlm@404: prop-display (view-proprioception) rlm@404: muscle-display (view-movement) rlm@449: {:keys [proprioception touch muscles body]} (worm) rlm@404: rlm@444: floor rlm@444: (box 5 1 5 :position (Vector3f. 0 -10 0) rlm@444: :mass 0 rlm@444: :texture "Textures/aurellem.png" rlm@444: :material "Common/MatDefs/Misc/Unshaded.j3md") rlm@445: timer (IsoTimer. 60) rlm@445: rlm@445: font (.loadFont (asset-manager) "Interface/Fonts/Console.fnt") rlm@445: worm-action (doto (BitmapText. font false) rlm@445: (.setSize 35) rlm@445: (.setColor (ColorRGBA/Black)))] rlm@399: rlm@404: (world rlm@449: (nodify [body floor]) rlm@404: (merge standard-debug-controls keybindings) rlm@404: (fn [world] rlm@445: (.setLocalTranslation rlm@445: worm-action 20 470 0) rlm@445: (.attachChild (.getGuiNode world) worm-action) rlm@445: rlm@444: (enable-good-shadows world) rlm@449: (.setShadowMode body RenderQueue$ShadowMode/CastAndReceive) rlm@444: (.setShadowMode floor RenderQueue$ShadowMode/Receive) rlm@444: rlm@444: (.setBackgroundColor (.getViewPort world) (ColorRGBA/White)) rlm@443: (.setDisplayStatView world false) rlm@443: (.setDisplayFps world false) rlm@404: (position-camera world view) rlm@407: (.setTimer world timer) rlm@449: ;;(display-dilated-time world timer) rlm@430: (when record rlm@445: (dir! record) rlm@404: (Capture/captureVideo rlm@404: world rlm@404: (dir! (File. record "main-view")))) rlm@451: (speed-up world 0.5) rlm@444: ;;(light-up-everything world) rlm@444: ) rlm@404: (fn [world tpf] rlm@410: (if (and end-frame (> (.getTime timer) end-frame)) rlm@407: (.stop world)) rlm@414: (let [muscle-data (vec (motor-control muscles)) rlm@449: proprioception-data (proprioception) rlm@415: touch-data (mapv #(% (.getRootNode world)) touch)] rlm@405: (when experiences rlm@405: (record-experience! rlm@405: experiences {:touch touch-data rlm@405: :proprioception proprioception-data rlm@418: :muscle muscle-data})) rlm@418: (when experience-watch rlm@445: (experience-watch @experiences worm-action)) rlm@404: (muscle-display rlm@405: muscle-data rlm@430: (when record (dir! (File. record "muscle")))) rlm@405: (prop-display rlm@405: proprioception-data rlm@430: (when record (dir! (File. record "proprio")))) rlm@405: (touch-display rlm@405: touch-data rlm@430: (when record (dir! (File. record "touch"))))))))) rlm@407: rlm@407: rlm@407: rlm@416: ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; rlm@416: ;;;;;;;; Phi-Space ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; rlm@416: ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; rlm@416: rlm@416: (defn generate-phi-space [] rlm@416: (let [experiences (atom [])] rlm@416: (run-world rlm@416: (apply-map rlm@416: worm-world rlm@416: (merge rlm@416: (worm-world-defaults) rlm@416: {:end-frame 700 rlm@416: :motor-control rlm@416: (motor-control-program worm-muscle-labels do-all-the-things) rlm@416: :experiences experiences}))) rlm@416: @experiences)) rlm@416: rlm@416: (defn bin [digits] rlm@416: (fn [angles] rlm@416: (->> angles rlm@416: (flatten) rlm@416: (map (juxt #(Math/sin %) #(Math/cos %))) rlm@416: (flatten) rlm@416: (mapv #(Math/round (* % (Math/pow 10 (dec digits)))))))) rlm@416: rlm@418: ;; k-nearest neighbors with spatial binning. Only returns a result if rlm@418: ;; the propriceptive data is within 10% of a previously recorded rlm@418: ;; result in all dimensions. rlm@416: (defn gen-phi-scan [phi-space] rlm@416: (let [bin-keys (map bin [3 2 1]) rlm@416: bin-maps rlm@417: (map (fn [bin-key] rlm@417: (group-by rlm@417: (comp bin-key :proprioception phi-space) rlm@417: (range (count phi-space)))) bin-keys) rlm@416: lookups (map (fn [bin-key bin-map] rlm@416: (fn [proprio] (bin-map (bin-key proprio)))) rlm@416: bin-keys bin-maps)] rlm@416: (fn lookup [proprio-data] rlm@419: (set (some #(% proprio-data) lookups))))) rlm@419: rlm@419: rlm@419: (defn longest-thread rlm@419: "Find the longest thread from phi-index-sets. The index sets should rlm@419: be ordered from most recent to least recent." rlm@419: [phi-index-sets] rlm@419: (loop [result '() rlm@419: [thread-bases & remaining :as phi-index-sets] phi-index-sets] rlm@419: (if (empty? phi-index-sets) rlm@420: (vec result) rlm@419: (let [threads rlm@419: (for [thread-base thread-bases] rlm@419: (loop [thread (list thread-base) rlm@419: remaining remaining] rlm@419: (let [next-index (dec (first thread))] rlm@419: (cond (empty? remaining) thread rlm@419: (contains? (first remaining) next-index) rlm@419: (recur rlm@419: (cons next-index thread) (rest remaining)) rlm@419: :else thread)))) rlm@419: longest-thread rlm@419: (reduce (fn [thread-a thread-b] rlm@419: (if (> (count thread-a) (count thread-b)) rlm@419: thread-a thread-b)) rlm@419: '(nil) rlm@419: threads)] rlm@419: (recur (concat longest-thread result) rlm@419: (drop (count longest-thread) phi-index-sets)))))) rlm@419: rlm@416: rlm@416: (defn init [] rlm@416: (def phi-space (generate-phi-space)) rlm@416: (def phi-scan (gen-phi-scan phi-space)) rlm@416: ) rlm@418: rlm@420: rlm@420: (defn infer-nils rlm@420: "Replace nils with the next available non-nil element in the rlm@420: sequence, or barring that, 0." rlm@420: [s] rlm@430: (loop [i (dec (count s)) rlm@430: v (transient s)] rlm@430: (if (zero? i) (persistent! v) rlm@430: (if-let [cur (v i)] rlm@430: (if (get v (dec i) 0) rlm@430: (recur (dec i) v) rlm@430: (recur (dec i) (assoc! v (dec i) cur))) rlm@430: (recur i (assoc! v i 0)))))) rlm@420: rlm@420: ;; tests rlm@420: rlm@420: ;;(infer-nils [1 nil 1 1]) [1 1 1 1] rlm@420: ;;(infer-nils [1 1 1 nil]) [1 1 1 0] rlm@420: ;;(infer-nils [nil 2 1 1]) [2 2 1 1] rlm@420: rlm@420: rlm@451: (defn empathy-demonstration [] rlm@420: (let [proprio (atom ())] rlm@420: (fn rlm@451: [experiences text] rlm@420: (let [phi-indices (phi-scan (:proprioception (peek experiences)))] rlm@420: (swap! proprio (partial cons phi-indices)) rlm@420: (let [exp-thread (longest-thread (take 300 @proprio)) rlm@451: empathy (mapv phi-space (infer-nils exp-thread))] rlm@420: (println-repl (vector:last-n exp-thread 22)) rlm@420: (cond rlm@451: (grand-circle? empathy) (.setText text "Grand Circle") rlm@451: (curled? empathy) (.setText text "Curled") rlm@451: (wiggling? empathy) (.setText text "Wiggling") rlm@451: (resting? empathy) (.setText text "Resting") rlm@451: :else (.setText text "Unknown"))))))) rlm@420: rlm@420: (defn init-interactive [] rlm@420: (def phi-space rlm@420: (let [experiences (atom [])] rlm@420: (run-world rlm@420: (apply-map rlm@420: worm-world rlm@420: (merge rlm@420: (worm-world-defaults) rlm@420: {:experiences experiences}))) rlm@420: @experiences)) rlm@420: (def phi-scan (gen-phi-scan phi-space))) rlm@420: rlm@451: (defn empathy-experiment-1 [record] rlm@451: (.start (worm-world :experience-watch (empathy-demonstration) rlm@451: :record record :worm worm*))) rlm@451: rlm@451: rlm@451: (def worm-action-label rlm@451: (juxt grand-circle? curled? wiggling?)) rlm@451: rlm@451: (defn compare-empathy-with-baseline [accuracy] rlm@451: (let [proprio (atom ())] rlm@451: (fn rlm@451: [experiences text] rlm@451: (let [phi-indices (phi-scan (:proprioception (peek experiences)))] rlm@451: (swap! proprio (partial cons phi-indices)) rlm@451: (let [exp-thread (longest-thread (take 300 @proprio)) rlm@451: empathy (mapv phi-space (infer-nils exp-thread)) rlm@451: experience-matches-empathy rlm@451: (= (worm-action-label experiences) rlm@451: (worm-action-label empathy))] rlm@451: (cond rlm@451: (grand-circle? empathy) (.setText text "Grand Circle") rlm@451: (curled? empathy) (.setText text "Curled") rlm@451: (wiggling? empathy) (.setText text "Wiggling") rlm@451: (resting? empathy) (.setText text "Resting") rlm@451: :else (.setText text "Unknown")) rlm@451: rlm@451: (println-repl experience-matches-empathy) rlm@451: (swap! accuracy #(conj % experience-matches-empathy))))))) rlm@526: rlm@526: (defn empathy-experiment-2 [] rlm@526: (.start (worm-world :experience-watch (compare-empathy-with-baseline rlm@526: (atom [])) rlm@526: :record false :worm worm*))) rlm@526: rlm@451: (defn accuracy [v] rlm@451: (float (/ (count (filter true? v)) (count v)))) rlm@451: rlm@451: (defn test-empathy-accuracy [] rlm@451: (let [res (atom [])] rlm@451: (run-world rlm@451: (worm-world :experience-watch rlm@451: (compare-empathy-with-baseline res) rlm@451: :worm worm*)) rlm@451: (accuracy @res))) rlm@451: rlm@451: rlm@451: rlm@516: (defn dylan-collect-bolts [longest-threads index-sets] rlm@516: (fn rlm@516: [experiences text] rlm@516: (let [phi-indices (phi-scan (:proprioception (peek experiences))) rlm@516: long-thread (longest-thread (vector:last-n @index-sets 300))] rlm@516: (swap! index-sets #(conj % phi-indices)) rlm@516: (swap! longest-threads #(conj % long-thread))))) rlm@451: rlm@451: rlm@451: