Mercurial > cortex

annotate org/worm_learn.clj @ 414:634795361af8

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
working on wiggling? stream predicate.
author Robert McIntyre <rlm@mit.edu>
date Wed, 19 Mar 2014 15:59:46 -0400
parents 54ef2e06c3ef
children af7945c27474
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@414 84 [[370 :d-flex 40]
rlm@414 85 [600 :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@400 109 ;; Normally, we'd use unsupervised/supervised machine learning to pick
rlm@400 110 ;; out the defining features of the different actions available to the
rlm@400 111 ;; worm. For this project, I am going to explicitely define functions
rlm@400 112 ;; that recognize curling and wiggling respectively. These functions
rlm@400 113 ;; are defined using all the information available from an embodied
rlm@400 114 ;; simulation of the action. Note how much easier they are to define
rlm@400 115 ;; than if I only had vision to work with. Things like scale/position
rlm@400 116 ;; invariance are complete non-issues here. This is the advantage of
rlm@400 117 ;; body-centered action recognition and what I hope to show with this
rlm@400 118 ;; thesis.
rlm@400 119
rlm@405 120
rlm@405 121 (defn straight?
rlm@405 122 "Is the worm straight?"
rlm@405 123 [experiences]
rlm@405 124 (every?
rlm@405 125 (fn [[_ _ bend]]
rlm@405 126 (< (Math/sin bend) 0.05))
rlm@405 127 (:proprioception (peek experiences))))
rlm@405 128
rlm@405 129 (defn curled?
rlm@405 130 "Is the worm curled up?"
rlm@405 131 [experiences]
rlm@405 132 (every?
rlm@405 133 (fn [[_ _ bend]]
rlm@405 134 (> (Math/sin bend) 0.64))
rlm@405 135 (:proprioception (peek experiences))))
rlm@405 136
rlm@405 137 (defn grand-circle?
rlm@405 138 "Does the worm form a majestic circle (one end touching the other)?"
rlm@405 139 [experiences]
rlm@405 140 (and (curled? experiences)
rlm@406 141 true)) ;; TODO: add code here.
rlm@405 142
rlm@405 143 (defn vector:last-n [v n]
rlm@405 144 (let [c (count v)]
rlm@405 145 (if (< c n) v
rlm@405 146 (subvec v (- c n) c))))
rlm@400 147
rlm@406 148 (defn touch-average [[coords touch]]
rlm@406 149 (/ (average (map first touch)) (average (map second touch))))
rlm@406 150
rlm@411 151 (defn rect-region [[x0 y0] [x1 y1]]
rlm@411 152 (vec
rlm@411 153 (for [x (range x0 (inc x1))
rlm@411 154 y (range y0 (inc y1))]
rlm@411 155 [x y])))
rlm@407 156
rlm@411 157 (def worm-segment-touch-bottom (rect-region [8 15] [14 22]))
rlm@407 158
rlm@411 159 (defn contact
rlm@411 160 "Determine how much contact a particular worm segment has with
rlm@411 161 other objects. Returns a value between 0 and 1, where 1 is full
rlm@411 162 contact and 0 is no contact."
rlm@411 163 [[coords contact :as touch]]
rlm@411 164 (-> (zipmap coords contact)
rlm@411 165 (select-keys worm-segment-touch-bottom)
rlm@411 166 (vals)
rlm@411 167 (#(map first %))
rlm@411 168 (average)
rlm@411 169 (* 10)
rlm@411 170 (- 1)
rlm@411 171 (Math/abs)))
rlm@406 172
rlm@413 173 (defn fft [nums]
rlm@414 174 (map
rlm@414 175 #(.getReal %)
rlm@414 176 (.transform
rlm@414 177 (FastFourierTransformer. DftNormalization/STANDARD)
rlm@414 178 (double-array nums) TransformType/FORWARD)))
rlm@413 179
rlm@413 180 (def indexed (partial map-indexed vector))
rlm@413 181
rlm@414 182 (defn max-indexed [s]
rlm@414 183 (first (sort-by (comp - second) (indexed s))))
rlm@414 184
rlm@414 185
rlm@400 186 (defn wiggling?
rlm@405 187 "Is the worm wiggling?"
rlm@405 188 [experiences]
rlm@414 189 (let [analysis-interval 0x40]
rlm@414 190 (when (> (count experiences) analysis-interval)
rlm@414 191 (let [a-flex 3
rlm@414 192 a-ex 2
rlm@414 193 muscle-activity
rlm@414 194 (map :muscle (vector:last-n experiences analysis-interval))
rlm@414 195 base-activity
rlm@414 196 (map #(- (% a-flex) (% a-ex)) muscle-activity)]
rlm@414 197 (= 2
rlm@414 198 (first
rlm@414 199 (max-indexed (map #(Math/abs %) (take 20 (fft base-activity))))))))))
rlm@414 200
rlm@414 201 ;; (println-repl
rlm@414 202 ;; (apply format "%d %.2f"
rlm@414 203 ;; (first (sort-by
rlm@414 204 ;; (comp - second)
rlm@414 205 ;; (indexed (take 20 ))))))))))
rlm@414 206
rlm@414 207 ;; (println-repl
rlm@414 208 ;; (apply
rlm@414 209 ;; format
rlm@414 210 ;; (apply str (repeat analysis-interval "%5.1f"))
rlm@414 211 ;; (fft base-activity)))
rlm@414 212
rlm@414 213 ;; ;;(println-repl (last base-activity))
rlm@414 214 ;; )))
rlm@414 215
rlm@414 216
rlm@400 217
rlm@400 218 (def standard-world-view
rlm@400 219 [(Vector3f. 4.207176, -3.7366982, 3.0816958)
rlm@400 220 (Quaternion. 0.11118768, 0.87678415, 0.24434438, -0.3989771)])
rlm@400 221
rlm@400 222 (def worm-side-view
rlm@400 223 [(Vector3f. 4.207176, -3.7366982, 3.0816958)
rlm@400 224 (Quaternion. -0.11555642, 0.88188726, -0.2854942, -0.3569518)])
rlm@400 225
rlm@400 226 (def degenerate-worm-view
rlm@400 227 [(Vector3f. -0.0708936, -8.570261, 2.6487997)
rlm@400 228 (Quaternion. -2.318909E-4, 0.9985348, 0.053941682, 0.004291452)])
rlm@399 229
rlm@404 230 (defn worm-world-defaults []
rlm@404 231 (let [direct-control (worm-direct-control worm-muscle-labels 40)]
rlm@404 232 {:view worm-side-view
rlm@404 233 :motor-control (:motor-control direct-control)
rlm@404 234 :keybindings (:keybindings direct-control)
rlm@405 235 :record nil
rlm@407 236 :experiences nil
rlm@407 237 :worm-model worm-model
rlm@407 238 :end-frame nil}))
rlm@407 239
rlm@404 240 (defn dir! [file]
rlm@410 241 (if-not (.exists file)
rlm@404 242 (.mkdir file))
rlm@404 243 file)
rlm@405 244
rlm@405 245 (defn record-experience! [experiences data]
rlm@405 246 (swap! experiences #(conj % data)))
rlm@405 247
rlm@399 248 (defn worm-world
rlm@407 249 [& {:keys [record motor-control keybindings view experiences
rlm@407 250 worm-model end-frame] :as settings}]
rlm@407 251 (let [{:keys [record motor-control keybindings view experiences
rlm@407 252 worm-model end-frame]}
rlm@404 253 (merge (worm-world-defaults) settings)
rlm@404 254 worm (doto (worm-model) (body!))
rlm@404 255 touch (touch! worm)
rlm@404 256 prop (proprioception! worm)
rlm@404 257 muscles (movement! worm)
rlm@404 258
rlm@404 259 touch-display (view-touch)
rlm@404 260 prop-display (view-proprioception)
rlm@404 261 muscle-display (view-movement)
rlm@404 262
rlm@404 263 floor (box 10 1 10 :position (Vector3f. 0 -10 0)
rlm@407 264 :color ColorRGBA/Gray :mass 0)
rlm@407 265 timer (IsoTimer. 60)]
rlm@399 266
rlm@404 267 (world
rlm@404 268 (nodify [worm floor])
rlm@404 269 (merge standard-debug-controls keybindings)
rlm@404 270 (fn [world]
rlm@404 271 (position-camera world view)
rlm@407 272 (.setTimer world timer)
rlm@407 273 (display-dilated-time world timer)
rlm@404 274 (if record
rlm@404 275 (Capture/captureVideo
rlm@404 276 world
rlm@404 277 (dir! (File. record "main-view"))))
rlm@404 278 (speed-up world)
rlm@404 279 (light-up-everything world))
rlm@404 280 (fn [world tpf]
rlm@410 281 (if (and end-frame (> (.getTime timer) end-frame))
rlm@407 282 (.stop world))
rlm@414 283 (let [muscle-data (vec (motor-control muscles))
rlm@405 284 proprioception-data (prop)
rlm@405 285 touch-data (map #(% (.getRootNode world)) touch)]
rlm@405 286 (when experiences
rlm@405 287 (record-experience!
rlm@405 288 experiences {:touch touch-data
rlm@405 289 :proprioception proprioception-data
rlm@405 290 :muscle muscle-data})
rlm@408 291 ;;(if (curled? @experiences) (println "Curled"))
rlm@406 292 ;;(if (straight? @experiences) (println "Straight"))
rlm@407 293 ;; (println-repl
rlm@407 294 ;; (apply format "%.2f %.2f %.2f %.2f %.2f\n"
rlm@411 295 ;; (map contact touch-data)))
rlm@414 296 (wiggling? @experiences)
rlm@405 297 )
rlm@404 298 (muscle-display
rlm@405 299 muscle-data
rlm@405 300 (if record (dir! (File. record "muscle"))))
rlm@405 301 (prop-display
rlm@405 302 proprioception-data
rlm@405 303 (if record (dir! (File. record "proprio"))))
rlm@405 304 (touch-display
rlm@405 305 touch-data
rlm@405 306 (if record (dir! (File. record "touch")))))))))
rlm@407 307
rlm@407 308
rlm@407 309