Mercurial > cortex

annotate org/worm_learn.clj @ 430:5205535237fb

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
fix skew in self-organizing-touch, work on thesis.
author Robert McIntyre <rlm@mit.edu>
date Sat, 22 Mar 2014 16:10:34 -0400
parents 7f3581dc58ff
children d3c5f9b70574
rev   line source
rlm@394 1 (ns org.aurellem.worm-learn
rlm@394 2 "General worm creation framework."
rlm@394 3 {:author "Robert McIntyre"}
rlm@394 4 (:use (cortex world util import body sense
rlm@408 5 hearing touch vision proprioception movement
rlm@408 6 test))
rlm@394 7 (:import (com.jme3.math ColorRGBA Vector3f))
rlm@394 8 (:import java.io.File)
rlm@394 9 (:import com.jme3.audio.AudioNode)
rlm@397 10 (:import com.aurellem.capture.RatchetTimer)
rlm@397 11 (:import (com.aurellem.capture Capture IsoTimer))
rlm@397 12 (:import (com.jme3.math Vector3f ColorRGBA)))
rlm@406 13
rlm@413 14 (import org.apache.commons.math3.transform.TransformType)
rlm@413 15 (import org.apache.commons.math3.transform.FastFourierTransformer)
rlm@413 16 (import org.apache.commons.math3.transform.DftNormalization)
rlm@413 17
rlm@406 18 (use 'clojure.pprint)
rlm@408 19 (use 'clojure.set)
rlm@394 20 (dorun (cortex.import/mega-import-jme3))
rlm@394 21 (rlm.rlm-commands/help)
rlm@394 22
rlm@400 23 (load-bullet)
rlm@394 24
rlm@399 25 (def hand "Models/test-creature/hand.blend")
rlm@394 26
rlm@399 27 (defn worm-model []
rlm@399 28 (load-blender-model "Models/worm/worm.blend"))
rlm@394 29
rlm@400 30 (def output-base (File. "/home/r/proj/cortex/render/worm-learn/curl"))
rlm@394 31
rlm@397 32
rlm@399 33 (defn motor-control-program
rlm@399 34 "Create a function which will execute the motor script"
rlm@406 35 [muscle-labels
rlm@399 36 script]
rlm@399 37 (let [current-frame (atom -1)
rlm@399 38 keyed-script (group-by first script)
rlm@399 39 current-forces (atom {}) ]
rlm@399 40 (fn [effectors]
rlm@399 41 (let [indexed-effectors (vec effectors)]
rlm@399 42 (dorun
rlm@399 43 (for [[_ part force] (keyed-script (swap! current-frame inc))]
rlm@399 44 (swap! current-forces (fn [m] (assoc m part force)))))
rlm@399 45 (doall (map (fn [effector power]
rlm@399 46 (effector (int power)))
rlm@399 47 effectors
rlm@406 48 (map #(@current-forces % 0) muscle-labels)))))))
rlm@397 49
rlm@404 50 (defn worm-direct-control
rlm@404 51 "Create keybindings and a muscle control program that will enable
rlm@404 52 the user to control the worm via the keyboard."
rlm@404 53 [muscle-labels activation-strength]
rlm@404 54 (let [strengths (mapv (fn [_] (atom 0)) muscle-labels)
rlm@404 55 activator
rlm@404 56 (fn [n]
rlm@404 57 (fn [world pressed?]
rlm@404 58 (let [strength (if pressed? activation-strength 0)]
rlm@404 59 (swap! (nth strengths n) (constantly strength)))))
rlm@404 60 activators
rlm@404 61 (map activator (range (count muscle-labels)))
rlm@404 62 worm-keys
rlm@404 63 ["key-f" "key-r"
rlm@404 64 "key-g" "key-t"
rlm@413 65 "key-h" "key-y"
rlm@404 66 "key-j" "key-u"
rlm@413 67 "key-k" "key-i"
rlm@413 68 "key-l" "key-o"]]
rlm@404 69 {:motor-control
rlm@404 70 (fn [effectors]
rlm@404 71 (doall
rlm@404 72 (map (fn [strength effector]
rlm@404 73 (effector (deref strength)))
rlm@404 74 strengths effectors)))
rlm@404 75 :keybindings
rlm@404 76 ;; assume muscles are listed in pairs and map them to keys.
rlm@404 77 (zipmap worm-keys activators)}))
rlm@400 78
rlm@400 79 ;; These are scripts that direct the worm to move in two radically
rlm@400 80 ;; different patterns -- a sinusoidal wiggling motion, and a curling
rlm@400 81 ;; motions that causes the worm to form a circle.
rlm@400 82
rlm@400 83 (def curl-script
rlm@415 84 [[150 :d-flex 40]
rlm@415 85 [250 :d-flex 0]])
rlm@400 86
rlm@400 87 (def period 18)
rlm@400 88
rlm@404 89 (def worm-muscle-labels
rlm@414 90 [:base-ex :base-flex
rlm@414 91 :a-ex :a-flex
rlm@414 92 :b-ex :b-flex
rlm@414 93 :c-ex :c-flex
rlm@414 94 :d-ex :d-flex])
rlm@399 95
rlm@399 96 (defn gen-wiggle [[flexor extensor :as muscle-pair] time-base]
rlm@399 97 (let [period period
rlm@399 98 power 45]
rlm@399 99 [[time-base flexor power]
rlm@399 100 [(+ time-base period) flexor 0]
rlm@399 101 [(+ time-base period 1) extensor power]
rlm@399 102 [(+ time-base (+ (* 2 period) 2)) extensor 0]]))
rlm@399 103
rlm@399 104 (def wiggle-script
rlm@414 105 (mapcat gen-wiggle (repeat 4000 [:a-ex :a-flex])
rlm@406 106 (range 100 1000000 (+ 3 (* period 2)))))
rlm@399 107
rlm@399 108
rlm@415 109 (defn shift-script [shift script]
rlm@415 110 (map (fn [[time label power]] [(+ time shift) label power])
rlm@415 111 script))
rlm@415 112
rlm@415 113 (def do-all-the-things
rlm@415 114 (concat
rlm@415 115 curl-script
rlm@415 116 [[300 :d-ex 40]
rlm@415 117 [320 :d-ex 0]]
rlm@415 118 (shift-script 280 (take 16 wiggle-script))))
rlm@415 119
rlm@400 120 ;; Normally, we'd use unsupervised/supervised machine learning to pick
rlm@400 121 ;; out the defining features of the different actions available to the
rlm@400 122 ;; worm. For this project, I am going to explicitely define functions
rlm@400 123 ;; that recognize curling and wiggling respectively. These functions
rlm@400 124 ;; are defined using all the information available from an embodied
rlm@400 125 ;; simulation of the action. Note how much easier they are to define
rlm@400 126 ;; than if I only had vision to work with. Things like scale/position
rlm@400 127 ;; invariance are complete non-issues here. This is the advantage of
rlm@400 128 ;; body-centered action recognition and what I hope to show with this
rlm@400 129 ;; thesis.
rlm@400 130
rlm@405 131
rlm@415 132 ;; curled? relies on proprioception, resting? relies on touch,
rlm@415 133 ;; wiggling? relies on a fourier analysis of muscle contraction, and
rlm@415 134 ;; grand-circle? relies on touch and reuses curled? as a gaurd.
rlm@405 135
rlm@405 136 (defn curled?
rlm@405 137 "Is the worm curled up?"
rlm@405 138 [experiences]
rlm@405 139 (every?
rlm@405 140 (fn [[_ _ bend]]
rlm@405 141 (> (Math/sin bend) 0.64))
rlm@405 142 (:proprioception (peek experiences))))
rlm@405 143
rlm@411 144 (defn rect-region [[x0 y0] [x1 y1]]
rlm@411 145 (vec
rlm@411 146 (for [x (range x0 (inc x1))
rlm@411 147 y (range y0 (inc y1))]
rlm@411 148 [x y])))
rlm@407 149
rlm@415 150 (def worm-segment-bottom (rect-region [8 15] [14 22]))
rlm@407 151
rlm@411 152 (defn contact
rlm@411 153 "Determine how much contact a particular worm segment has with
rlm@411 154 other objects. Returns a value between 0 and 1, where 1 is full
rlm@411 155 contact and 0 is no contact."
rlm@415 156 [touch-region [coords contact :as touch]]
rlm@411 157 (-> (zipmap coords contact)
rlm@415 158 (select-keys touch-region)
rlm@411 159 (vals)
rlm@411 160 (#(map first %))
rlm@411 161 (average)
rlm@411 162 (* 10)
rlm@411 163 (- 1)
rlm@411 164 (Math/abs)))
rlm@406 165
rlm@415 166 (defn resting?
rlm@415 167 "Is the worm straight?"
rlm@415 168 [experiences]
rlm@415 169 (every?
rlm@415 170 (fn [touch-data]
rlm@415 171 (< 0.9 (contact worm-segment-bottom touch-data)))
rlm@415 172 (:touch (peek experiences))))
rlm@415 173
rlm@415 174 (defn vector:last-n [v n]
rlm@415 175 (let [c (count v)]
rlm@415 176 (if (< c n) v
rlm@415 177 (subvec v (- c n) c))))
rlm@415 178
rlm@413 179 (defn fft [nums]
rlm@414 180 (map
rlm@414 181 #(.getReal %)
rlm@414 182 (.transform
rlm@414 183 (FastFourierTransformer. DftNormalization/STANDARD)
rlm@414 184 (double-array nums) TransformType/FORWARD)))
rlm@413 185
rlm@413 186 (def indexed (partial map-indexed vector))
rlm@413 187
rlm@414 188 (defn max-indexed [s]
rlm@414 189 (first (sort-by (comp - second) (indexed s))))
rlm@414 190
rlm@400 191 (defn wiggling?
rlm@405 192 "Is the worm wiggling?"
rlm@405 193 [experiences]
rlm@414 194 (let [analysis-interval 0x40]
rlm@414 195 (when (> (count experiences) analysis-interval)
rlm@414 196 (let [a-flex 3
rlm@414 197 a-ex 2
rlm@414 198 muscle-activity
rlm@414 199 (map :muscle (vector:last-n experiences analysis-interval))
rlm@414 200 base-activity
rlm@414 201 (map #(- (% a-flex) (% a-ex)) muscle-activity)]
rlm@414 202 (= 2
rlm@414 203 (first
rlm@415 204 (max-indexed
rlm@415 205 (map #(Math/abs %)
rlm@415 206 (take 20 (fft base-activity))))))))))
rlm@414 207
rlm@415 208 (def worm-segment-bottom-tip (rect-region [15 15] [22 22]))
rlm@414 209
rlm@415 210 (def worm-segment-top-tip (rect-region [0 15] [7 22]))
rlm@414 211
rlm@415 212 (defn grand-circle?
rlm@415 213 "Does the worm form a majestic circle (one end touching the other)?"
rlm@415 214 [experiences]
rlm@420 215 (and (curled? experiences)
rlm@415 216 (let [worm-touch (:touch (peek experiences))
rlm@415 217 tail-touch (worm-touch 0)
rlm@415 218 head-touch (worm-touch 4)]
rlm@415 219 (and (< 0.55 (contact worm-segment-bottom-tip tail-touch))
rlm@415 220 (< 0.55 (contact worm-segment-top-tip head-touch))))))
rlm@400 221
rlm@418 222
rlm@418 223 (declare phi-space phi-scan)
rlm@418 224
rlm@418 225 (defn debug-experience
rlm@418 226 [experiences]
rlm@418 227 (cond
rlm@418 228 (grand-circle? experiences) (println "Grand Circle")
rlm@418 229 (curled? experiences) (println "Curled")
rlm@418 230 (wiggling? experiences) (println "Wiggling")
rlm@418 231 (resting? experiences) (println "Resting")))
rlm@418 232
rlm@418 233
rlm@400 234 (def standard-world-view
rlm@400 235 [(Vector3f. 4.207176, -3.7366982, 3.0816958)
rlm@400 236 (Quaternion. 0.11118768, 0.87678415, 0.24434438, -0.3989771)])
rlm@400 237
rlm@400 238 (def worm-side-view
rlm@400 239 [(Vector3f. 4.207176, -3.7366982, 3.0816958)
rlm@400 240 (Quaternion. -0.11555642, 0.88188726, -0.2854942, -0.3569518)])
rlm@400 241
rlm@400 242 (def degenerate-worm-view
rlm@400 243 [(Vector3f. -0.0708936, -8.570261, 2.6487997)
rlm@400 244 (Quaternion. -2.318909E-4, 0.9985348, 0.053941682, 0.004291452)])
rlm@399 245
rlm@404 246 (defn worm-world-defaults []
rlm@404 247 (let [direct-control (worm-direct-control worm-muscle-labels 40)]
rlm@430 248 (merge direct-control
rlm@430 249 {:view worm-side-view
rlm@430 250 :record nil
rlm@430 251 :experiences (atom [])
rlm@430 252 :experience-watch debug-experience
rlm@430 253 :worm-model worm-model
rlm@430 254 :end-frame nil})))
rlm@407 255
rlm@404 256 (defn dir! [file]
rlm@410 257 (if-not (.exists file)
rlm@404 258 (.mkdir file))
rlm@404 259 file)
rlm@405 260
rlm@405 261 (defn record-experience! [experiences data]
rlm@405 262 (swap! experiences #(conj % data)))
rlm@405 263
rlm@399 264 (defn worm-world
rlm@407 265 [& {:keys [record motor-control keybindings view experiences
rlm@418 266 worm-model end-frame experience-watch] :as settings}]
rlm@407 267 (let [{:keys [record motor-control keybindings view experiences
rlm@418 268 worm-model end-frame experience-watch]}
rlm@404 269 (merge (worm-world-defaults) settings)
rlm@404 270 worm (doto (worm-model) (body!))
rlm@404 271 touch (touch! worm)
rlm@404 272 prop (proprioception! worm)
rlm@404 273 muscles (movement! worm)
rlm@404 274
rlm@404 275 touch-display (view-touch)
rlm@404 276 prop-display (view-proprioception)
rlm@404 277 muscle-display (view-movement)
rlm@404 278
rlm@404 279 floor (box 10 1 10 :position (Vector3f. 0 -10 0)
rlm@407 280 :color ColorRGBA/Gray :mass 0)
rlm@407 281 timer (IsoTimer. 60)]
rlm@399 282
rlm@404 283 (world
rlm@404 284 (nodify [worm floor])
rlm@404 285 (merge standard-debug-controls keybindings)
rlm@404 286 (fn [world]
rlm@404 287 (position-camera world view)
rlm@407 288 (.setTimer world timer)
rlm@407 289 (display-dilated-time world timer)
rlm@430 290 (when record
rlm@404 291 (Capture/captureVideo
rlm@404 292 world
rlm@404 293 (dir! (File. record "main-view"))))
rlm@404 294 (speed-up world)
rlm@404 295 (light-up-everything world))
rlm@404 296 (fn [world tpf]
rlm@410 297 (if (and end-frame (> (.getTime timer) end-frame))
rlm@407 298 (.stop world))
rlm@414 299 (let [muscle-data (vec (motor-control muscles))
rlm@405 300 proprioception-data (prop)
rlm@415 301 touch-data (mapv #(% (.getRootNode world)) touch)]
rlm@405 302 (when experiences
rlm@405 303 (record-experience!
rlm@405 304 experiences {:touch touch-data
rlm@405 305 :proprioception proprioception-data
rlm@418 306 :muscle muscle-data}))
rlm@418 307 (when experience-watch
rlm@418 308 (experience-watch @experiences))
rlm@404 309 (muscle-display
rlm@405 310 muscle-data
rlm@430 311 (when record (dir! (File. record "muscle"))))
rlm@405 312 (prop-display
rlm@405 313 proprioception-data
rlm@430 314 (when record (dir! (File. record "proprio"))))
rlm@405 315 (touch-display
rlm@405 316 touch-data
rlm@430 317 (when record (dir! (File. record "touch")))))))))
rlm@407 318
rlm@407 319
rlm@407 320
rlm@416 321 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@416 322 ;;;;;;;; Phi-Space ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@416 323 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@416 324
rlm@416 325 (defn generate-phi-space []
rlm@416 326 (let [experiences (atom [])]
rlm@416 327 (run-world
rlm@416 328 (apply-map
rlm@416 329 worm-world
rlm@416 330 (merge
rlm@416 331 (worm-world-defaults)
rlm@416 332 {:end-frame 700
rlm@416 333 :motor-control
rlm@416 334 (motor-control-program worm-muscle-labels do-all-the-things)
rlm@416 335 :experiences experiences})))
rlm@416 336 @experiences))
rlm@416 337
rlm@416 338 (defn bin [digits]
rlm@416 339 (fn [angles]
rlm@416 340 (->> angles
rlm@416 341 (flatten)
rlm@416 342 (map (juxt #(Math/sin %) #(Math/cos %)))
rlm@416 343 (flatten)
rlm@416 344 (mapv #(Math/round (* % (Math/pow 10 (dec digits))))))))
rlm@416 345
rlm@418 346 ;; k-nearest neighbors with spatial binning. Only returns a result if
rlm@418 347 ;; the propriceptive data is within 10% of a previously recorded
rlm@418 348 ;; result in all dimensions.
rlm@416 349 (defn gen-phi-scan [phi-space]
rlm@416 350 (let [bin-keys (map bin [3 2 1])
rlm@416 351 bin-maps
rlm@417 352 (map (fn [bin-key]
rlm@417 353 (group-by
rlm@417 354 (comp bin-key :proprioception phi-space)
rlm@417 355 (range (count phi-space)))) bin-keys)
rlm@416 356 lookups (map (fn [bin-key bin-map]
rlm@416 357 (fn [proprio] (bin-map (bin-key proprio))))
rlm@416 358 bin-keys bin-maps)]
rlm@416 359 (fn lookup [proprio-data]
rlm@419 360 (set (some #(% proprio-data) lookups)))))
rlm@419 361
rlm@419 362
rlm@419 363 (defn longest-thread
rlm@419 364 "Find the longest thread from phi-index-sets. The index sets should
rlm@419 365 be ordered from most recent to least recent."
rlm@419 366 [phi-index-sets]
rlm@419 367 (loop [result '()
rlm@419 368 [thread-bases & remaining :as phi-index-sets] phi-index-sets]
rlm@419 369 (if (empty? phi-index-sets)
rlm@420 370 (vec result)
rlm@419 371 (let [threads
rlm@419 372 (for [thread-base thread-bases]
rlm@419 373 (loop [thread (list thread-base)
rlm@419 374 remaining remaining]
rlm@419 375 (let [next-index (dec (first thread))]
rlm@419 376 (cond (empty? remaining) thread
rlm@419 377 (contains? (first remaining) next-index)
rlm@419 378 (recur
rlm@419 379 (cons next-index thread) (rest remaining))
rlm@419 380 :else thread))))
rlm@419 381 longest-thread
rlm@419 382 (reduce (fn [thread-a thread-b]
rlm@419 383 (if (> (count thread-a) (count thread-b))
rlm@419 384 thread-a thread-b))
rlm@419 385 '(nil)
rlm@419 386 threads)]
rlm@419 387 (recur (concat longest-thread result)
rlm@419 388 (drop (count longest-thread) phi-index-sets))))))
rlm@419 389
rlm@416 390
rlm@416 391 (defn init []
rlm@416 392 (def phi-space (generate-phi-space))
rlm@416 393 (def phi-scan (gen-phi-scan phi-space))
rlm@416 394 )
rlm@418 395
rlm@430 396 ;; (defn infer-nils-dyl [s]
rlm@430 397 ;; (loop [closed ()
rlm@430 398 ;; open s
rlm@430 399 ;; anchor 0]
rlm@430 400 ;; (if-not (empty? open)
rlm@430 401 ;; (recur (conj closed
rlm@430 402 ;; (or (peek open)
rlm@430 403 ;; anchor))
rlm@430 404 ;; (pop open)
rlm@430 405 ;; (or (peek open) anchor))
rlm@430 406 ;; closed)))
rlm@430 407
rlm@430 408 ;; (defn infer-nils [s]
rlm@430 409 ;; (for [i (range (count s))]
rlm@430 410 ;; (or (get s i)
rlm@430 411 ;; (some (comp not nil?) (vector:last-n (- (count s) i)))
rlm@430 412 ;; 0)))
rlm@420 413
rlm@420 414
rlm@420 415 (defn infer-nils
rlm@420 416 "Replace nils with the next available non-nil element in the
rlm@420 417 sequence, or barring that, 0."
rlm@420 418 [s]
rlm@430 419 (loop [i (dec (count s))
rlm@430 420 v (transient s)]
rlm@430 421 (if (zero? i) (persistent! v)
rlm@430 422 (if-let [cur (v i)]
rlm@430 423 (if (get v (dec i) 0)
rlm@430 424 (recur (dec i) v)
rlm@430 425 (recur (dec i) (assoc! v (dec i) cur)))
rlm@430 426 (recur i (assoc! v i 0))))))
rlm@420 427
rlm@420 428 ;; tests
rlm@420 429
rlm@420 430 ;;(infer-nils [1 nil 1 1]) [1 1 1 1]
rlm@420 431 ;;(infer-nils [1 1 1 nil]) [1 1 1 0]
rlm@420 432 ;;(infer-nils [nil 2 1 1]) [2 2 1 1]
rlm@420 433
rlm@420 434
rlm@420 435 (defn debug-experience-phi []
rlm@420 436 (let [proprio (atom ())]
rlm@420 437 (fn
rlm@420 438 [experiences]
rlm@420 439 (let [phi-indices (phi-scan (:proprioception (peek experiences)))]
rlm@420 440 (swap! proprio (partial cons phi-indices))
rlm@420 441 (let [exp-thread (longest-thread (take 300 @proprio))
rlm@420 442 phi-loop (mapv phi-space (infer-nils exp-thread))]
rlm@420 443 (println-repl (vector:last-n exp-thread 22))
rlm@420 444 (cond
rlm@420 445 (grand-circle? phi-loop) (println "Grand Circle")
rlm@420 446 (curled? phi-loop) (println "Curled")
rlm@420 447 (wiggling? phi-loop) (println "Wiggling")
rlm@420 448 (resting? phi-loop) (println "Resting")
rlm@420 449 :else (println "Unknown")))))))
rlm@420 450
rlm@420 451
rlm@420 452 (defn init-interactive []
rlm@420 453 (def phi-space
rlm@420 454 (let [experiences (atom [])]
rlm@420 455 (run-world
rlm@420 456 (apply-map
rlm@420 457 worm-world
rlm@420 458 (merge
rlm@420 459 (worm-world-defaults)
rlm@420 460 {:experiences experiences})))
rlm@420 461 @experiences))
rlm@420 462 (def phi-scan (gen-phi-scan phi-space)))
rlm@420 463
rlm@420 464
rlm@420 465 (defn run-experiment-1 []
rlm@420 466 (.start (worm-world :experience-watch (debug-experience-phi))))