Mercurial > cortex
view org/worm_learn.clj @ 443:d3c5f9b70574
workling on thesis render.
author | Robert McIntyre <rlm@mit.edu> |
---|---|
date | Tue, 25 Mar 2014 00:20:01 -0400 |
parents | 5205535237fb |
children | ea0bcd47d55b |
line wrap: on
line source
1 (ns org.aurellem.worm-learn2 "General worm creation framework."3 {:author "Robert McIntyre"}4 (:use (cortex world util import body sense5 hearing touch vision proprioception movement6 test))7 (:import (com.jme3.math ColorRGBA Vector3f))8 (:import java.io.File)9 (:import com.jme3.audio.AudioNode)10 (:import com.aurellem.capture.RatchetTimer)11 (:import (com.aurellem.capture Capture IsoTimer))12 (:import (com.jme3.math Vector3f ColorRGBA)))14 (import org.apache.commons.math3.transform.TransformType)15 (import org.apache.commons.math3.transform.FastFourierTransformer)16 (import org.apache.commons.math3.transform.DftNormalization)18 (use 'clojure.pprint)19 (use 'clojure.set)20 (dorun (cortex.import/mega-import-jme3))21 (rlm.rlm-commands/help)23 (load-bullet)25 (def hand "Models/test-creature/hand.blend")27 (defn worm-model []28 (load-blender-model "Models/worm/worm.blend"))30 (def output-base (File. "/home/r/proj/cortex/render/worm-learn/curl"))33 (defn motor-control-program34 "Create a function which will execute the motor script"35 [muscle-labels36 script]37 (let [current-frame (atom -1)38 keyed-script (group-by first script)39 current-forces (atom {}) ]40 (fn [effectors]41 (let [indexed-effectors (vec effectors)]42 (dorun43 (for [[_ part force] (keyed-script (swap! current-frame inc))]44 (swap! current-forces (fn [m] (assoc m part force)))))45 (doall (map (fn [effector power]46 (effector (int power)))47 effectors48 (map #(@current-forces % 0) muscle-labels)))))))50 (defn worm-direct-control51 "Create keybindings and a muscle control program that will enable52 the user to control the worm via the keyboard."53 [muscle-labels activation-strength]54 (let [strengths (mapv (fn [_] (atom 0)) muscle-labels)55 activator56 (fn [n]57 (fn [world pressed?]58 (let [strength (if pressed? activation-strength 0)]59 (swap! (nth strengths n) (constantly strength)))))60 activators61 (map activator (range (count muscle-labels)))62 worm-keys63 ["key-f" "key-r"64 "key-g" "key-t"65 "key-h" "key-y"66 "key-j" "key-u"67 "key-k" "key-i"68 "key-l" "key-o"]]69 {:motor-control70 (fn [effectors]71 (doall72 (map (fn [strength effector]73 (effector (deref strength)))74 strengths effectors)))75 :keybindings76 ;; assume muscles are listed in pairs and map them to keys.77 (zipmap worm-keys activators)}))79 ;; These are scripts that direct the worm to move in two radically80 ;; different patterns -- a sinusoidal wiggling motion, and a curling81 ;; motions that causes the worm to form a circle.83 (def curl-script84 [[150 :d-flex 40]85 [250 :d-flex 0]])87 (def period 18)89 (def worm-muscle-labels90 [:base-ex :base-flex91 :a-ex :a-flex92 :b-ex :b-flex93 :c-ex :c-flex94 :d-ex :d-flex])96 (defn gen-wiggle [[flexor extensor :as muscle-pair] time-base]97 (let [period period98 power 45]99 [[time-base flexor power]100 [(+ time-base period) flexor 0]101 [(+ time-base period 1) extensor power]102 [(+ time-base (+ (* 2 period) 2)) extensor 0]]))104 (def wiggle-script105 (mapcat gen-wiggle (repeat 4000 [:a-ex :a-flex])106 (range 100 1000000 (+ 3 (* period 2)))))109 (defn shift-script [shift script]110 (map (fn [[time label power]] [(+ time shift) label power])111 script))113 (def do-all-the-things114 (concat115 curl-script116 [[300 :d-ex 40]117 [320 :d-ex 0]]118 (shift-script 280 (take 16 wiggle-script))))120 ;; Normally, we'd use unsupervised/supervised machine learning to pick121 ;; out the defining features of the different actions available to the122 ;; worm. For this project, I am going to explicitely define functions123 ;; that recognize curling and wiggling respectively. These functions124 ;; are defined using all the information available from an embodied125 ;; simulation of the action. Note how much easier they are to define126 ;; than if I only had vision to work with. Things like scale/position127 ;; invariance are complete non-issues here. This is the advantage of128 ;; body-centered action recognition and what I hope to show with this129 ;; thesis.132 ;; curled? relies on proprioception, resting? relies on touch,133 ;; wiggling? relies on a fourier analysis of muscle contraction, and134 ;; grand-circle? relies on touch and reuses curled? as a gaurd.136 (defn curled?137 "Is the worm curled up?"138 [experiences]139 (every?140 (fn [[_ _ bend]]141 (> (Math/sin bend) 0.64))142 (:proprioception (peek experiences))))144 (defn rect-region [[x0 y0] [x1 y1]]145 (vec146 (for [x (range x0 (inc x1))147 y (range y0 (inc y1))]148 [x y])))150 (def worm-segment-bottom (rect-region [8 15] [14 22]))152 (defn contact153 "Determine how much contact a particular worm segment has with154 other objects. Returns a value between 0 and 1, where 1 is full155 contact and 0 is no contact."156 [touch-region [coords contact :as touch]]157 (-> (zipmap coords contact)158 (select-keys touch-region)159 (vals)160 (#(map first %))161 (average)162 (* 10)163 (- 1)164 (Math/abs)))166 (defn resting?167 "Is the worm resting on the ground?"168 [experiences]169 (every?170 (fn [touch-data]171 (< 0.9 (contact worm-segment-bottom touch-data)))172 (:touch (peek experiences))))174 (defn vector:last-n [v n]175 (let [c (count v)]176 (if (< c n) v177 (subvec v (- c n) c))))179 (defn fft [nums]180 (map181 #(.getReal %)182 (.transform183 (FastFourierTransformer. DftNormalization/STANDARD)184 (double-array nums) TransformType/FORWARD)))186 (def indexed (partial map-indexed vector))188 (defn max-indexed [s]189 (first (sort-by (comp - second) (indexed s))))191 (defn wiggling?192 "Is the worm wiggling?"193 [experiences]194 (let [analysis-interval 0x40]195 (when (> (count experiences) analysis-interval)196 (let [a-flex 3197 a-ex 2198 muscle-activity199 (map :muscle (vector:last-n experiences analysis-interval))200 base-activity201 (map #(- (% a-flex) (% a-ex)) muscle-activity)]202 (= 2203 (first204 (max-indexed205 (map #(Math/abs %)206 (take 20 (fft base-activity))))))))))208 (def worm-segment-bottom-tip (rect-region [15 15] [22 22]))210 (def worm-segment-top-tip (rect-region [0 15] [7 22]))212 (defn grand-circle?213 "Does the worm form a majestic circle (one end touching the other)?"214 [experiences]215 (and (curled? experiences)216 (let [worm-touch (:touch (peek experiences))217 tail-touch (worm-touch 0)218 head-touch (worm-touch 4)]219 (and (< 0.55 (contact worm-segment-bottom-tip tail-touch))220 (< 0.55 (contact worm-segment-top-tip head-touch))))))223 (declare phi-space phi-scan)225 (defn debug-experience226 [experiences]227 (cond228 (grand-circle? experiences) (println "Grand Circle")229 (curled? experiences) (println "Curled")230 (wiggling? experiences) (println "Wiggling")231 (resting? experiences) (println "Resting")))234 (def standard-world-view235 [(Vector3f. 4.207176, -3.7366982, 3.0816958)236 (Quaternion. 0.11118768, 0.87678415, 0.24434438, -0.3989771)])238 (def worm-side-view239 [(Vector3f. 4.207176, -3.7366982, 3.0816958)240 (Quaternion. -0.11555642, 0.88188726, -0.2854942, -0.3569518)])242 (def degenerate-worm-view243 [(Vector3f. -0.0708936, -8.570261, 2.6487997)244 (Quaternion. -2.318909E-4, 0.9985348, 0.053941682, 0.004291452)])246 (defn worm-world-defaults []247 (let [direct-control (worm-direct-control worm-muscle-labels 40)]248 (merge direct-control249 {:view worm-side-view250 :record nil251 :experiences (atom [])252 :experience-watch debug-experience253 :worm-model worm-model254 :end-frame nil})))256 (defn dir! [file]257 (if-not (.exists file)258 (.mkdir file))259 file)261 (defn record-experience! [experiences data]262 (swap! experiences #(conj % data)))266 (defn worm-world267 [& {:keys [record motor-control keybindings view experiences268 worm-model end-frame experience-watch] :as settings}]269 (let [{:keys [record motor-control keybindings view experiences270 worm-model end-frame experience-watch]}271 (merge (worm-world-defaults) settings)272 worm (doto (worm-model) (body!))273 touch (touch! worm)274 prop (proprioception! worm)275 muscles (movement! worm)277 touch-display (view-touch)278 prop-display (view-proprioception)279 muscle-display (view-movement)281 floor (box 10 1 10 :position (Vector3f. 0 -10 0)282 :color ColorRGBA/Gray :mass 0283 :texture "Textures/greenGrid.png")284 timer (IsoTimer. 60)]286 (world287 (nodify [worm floor])288 (merge standard-debug-controls keybindings)289 (fn [world]290 (.setDisplayStatView world false)291 (.setDisplayFps world false)292 (position-camera world view)293 (.setTimer world timer)294 (display-dilated-time world timer)295 (when record296 (Capture/captureVideo297 world298 (dir! (File. record "main-view"))))299 (speed-up world)300 (light-up-everything world))301 (fn [world tpf]302 (if (and end-frame (> (.getTime timer) end-frame))303 (.stop world))304 (let [muscle-data (vec (motor-control muscles))305 proprioception-data (prop)306 touch-data (mapv #(% (.getRootNode world)) touch)]307 (when experiences308 (record-experience!309 experiences {:touch touch-data310 :proprioception proprioception-data311 :muscle muscle-data}))312 (when experience-watch313 (experience-watch @experiences))314 (muscle-display315 muscle-data316 (when record (dir! (File. record "muscle"))))317 (prop-display318 proprioception-data319 (when record (dir! (File. record "proprio"))))320 (touch-display321 touch-data322 (when record (dir! (File. record "touch")))))))))326 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;327 ;;;;;;;; Phi-Space ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;328 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;330 (defn generate-phi-space []331 (let [experiences (atom [])]332 (run-world333 (apply-map334 worm-world335 (merge336 (worm-world-defaults)337 {:end-frame 700338 :motor-control339 (motor-control-program worm-muscle-labels do-all-the-things)340 :experiences experiences})))341 @experiences))343 (defn bin [digits]344 (fn [angles]345 (->> angles346 (flatten)347 (map (juxt #(Math/sin %) #(Math/cos %)))348 (flatten)349 (mapv #(Math/round (* % (Math/pow 10 (dec digits))))))))351 ;; k-nearest neighbors with spatial binning. Only returns a result if352 ;; the propriceptive data is within 10% of a previously recorded353 ;; result in all dimensions.354 (defn gen-phi-scan [phi-space]355 (let [bin-keys (map bin [3 2 1])356 bin-maps357 (map (fn [bin-key]358 (group-by359 (comp bin-key :proprioception phi-space)360 (range (count phi-space)))) bin-keys)361 lookups (map (fn [bin-key bin-map]362 (fn [proprio] (bin-map (bin-key proprio))))363 bin-keys bin-maps)]364 (fn lookup [proprio-data]365 (set (some #(% proprio-data) lookups)))))368 (defn longest-thread369 "Find the longest thread from phi-index-sets. The index sets should370 be ordered from most recent to least recent."371 [phi-index-sets]372 (loop [result '()373 [thread-bases & remaining :as phi-index-sets] phi-index-sets]374 (if (empty? phi-index-sets)375 (vec result)376 (let [threads377 (for [thread-base thread-bases]378 (loop [thread (list thread-base)379 remaining remaining]380 (let [next-index (dec (first thread))]381 (cond (empty? remaining) thread382 (contains? (first remaining) next-index)383 (recur384 (cons next-index thread) (rest remaining))385 :else thread))))386 longest-thread387 (reduce (fn [thread-a thread-b]388 (if (> (count thread-a) (count thread-b))389 thread-a thread-b))390 '(nil)391 threads)]392 (recur (concat longest-thread result)393 (drop (count longest-thread) phi-index-sets))))))396 (defn init []397 (def phi-space (generate-phi-space))398 (def phi-scan (gen-phi-scan phi-space))399 )401 ;; (defn infer-nils-dyl [s]402 ;; (loop [closed ()403 ;; open s404 ;; anchor 0]405 ;; (if-not (empty? open)406 ;; (recur (conj closed407 ;; (or (peek open)408 ;; anchor))409 ;; (pop open)410 ;; (or (peek open) anchor))411 ;; closed)))413 ;; (defn infer-nils [s]414 ;; (for [i (range (count s))]415 ;; (or (get s i)416 ;; (some (comp not nil?) (vector:last-n (- (count s) i)))417 ;; 0)))420 (defn infer-nils421 "Replace nils with the next available non-nil element in the422 sequence, or barring that, 0."423 [s]424 (loop [i (dec (count s))425 v (transient s)]426 (if (zero? i) (persistent! v)427 (if-let [cur (v i)]428 (if (get v (dec i) 0)429 (recur (dec i) v)430 (recur (dec i) (assoc! v (dec i) cur)))431 (recur i (assoc! v i 0))))))433 ;; tests435 ;;(infer-nils [1 nil 1 1]) [1 1 1 1]436 ;;(infer-nils [1 1 1 nil]) [1 1 1 0]437 ;;(infer-nils [nil 2 1 1]) [2 2 1 1]440 (defn debug-experience-phi []441 (let [proprio (atom ())]442 (fn443 [experiences]444 (let [phi-indices (phi-scan (:proprioception (peek experiences)))]445 (swap! proprio (partial cons phi-indices))446 (let [exp-thread (longest-thread (take 300 @proprio))447 phi-loop (mapv phi-space (infer-nils exp-thread))]448 (println-repl (vector:last-n exp-thread 22))449 (cond450 (grand-circle? phi-loop) (println "Grand Circle")451 (curled? phi-loop) (println "Curled")452 (wiggling? phi-loop) (println "Wiggling")453 (resting? phi-loop) (println "Resting")454 :else (println "Unknown")))))))457 (defn init-interactive []458 (def phi-space459 (let [experiences (atom [])]460 (run-world461 (apply-map462 worm-world463 (merge464 (worm-world-defaults)465 {:experiences experiences})))466 @experiences))467 (def phi-scan (gen-phi-scan phi-space)))470 (defn run-experiment-1 []471 (.start (worm-world :experience-watch (debug-experience-phi))))