Mercurial > cortex

annotate org/worm_learn.clj @ 409:ea524e4d8f8d

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
save.
author Robert McIntyre <rlm@mit.edu>
date Tue, 18 Mar 2014 21:44:15 -0400
parents 3b4012b42611
children e6a7e80f885a
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@406 14 (use 'clojure.pprint)
rlm@408 15 (use 'clojure.set)
rlm@394 16 (dorun (cortex.import/mega-import-jme3))
rlm@394 17 (rlm.rlm-commands/help)
rlm@394 18
rlm@400 19 (load-bullet)
rlm@394 20
rlm@399 21 (def hand "Models/test-creature/hand.blend")
rlm@394 22
rlm@399 23 (defn worm-model []
rlm@399 24 (load-blender-model "Models/worm/worm.blend"))
rlm@394 25
rlm@400 26 (def output-base (File. "/home/r/proj/cortex/render/worm-learn/curl"))
rlm@394 27
rlm@397 28
rlm@399 29 (defn motor-control-program
rlm@399 30 "Create a function which will execute the motor script"
rlm@406 31 [muscle-labels
rlm@399 32 script]
rlm@399 33 (let [current-frame (atom -1)
rlm@399 34 keyed-script (group-by first script)
rlm@399 35 current-forces (atom {}) ]
rlm@399 36 (fn [effectors]
rlm@399 37 (let [indexed-effectors (vec effectors)]
rlm@399 38 (dorun
rlm@399 39 (for [[_ part force] (keyed-script (swap! current-frame inc))]
rlm@399 40 (swap! current-forces (fn [m] (assoc m part force)))))
rlm@399 41 (doall (map (fn [effector power]
rlm@399 42 (effector (int power)))
rlm@399 43 effectors
rlm@406 44 (map #(@current-forces % 0) muscle-labels)))))))
rlm@397 45
rlm@404 46 (defn worm-direct-control
rlm@404 47 "Create keybindings and a muscle control program that will enable
rlm@404 48 the user to control the worm via the keyboard."
rlm@404 49 [muscle-labels activation-strength]
rlm@404 50 (let [strengths (mapv (fn [_] (atom 0)) muscle-labels)
rlm@404 51 activator
rlm@404 52 (fn [n]
rlm@404 53 (fn [world pressed?]
rlm@404 54 (let [strength (if pressed? activation-strength 0)]
rlm@404 55 (swap! (nth strengths n) (constantly strength)))))
rlm@404 56 activators
rlm@404 57 (map activator (range (count muscle-labels)))
rlm@404 58 worm-keys
rlm@404 59 ["key-f" "key-r"
rlm@404 60 "key-g" "key-t"
rlm@404 61 "key-y" "key-h"
rlm@404 62 "key-j" "key-u"
rlm@404 63 "key-i" "key-k"
rlm@404 64 "key-o" "key-l"]]
rlm@404 65 {:motor-control
rlm@404 66 (fn [effectors]
rlm@404 67 (doall
rlm@404 68 (map (fn [strength effector]
rlm@404 69 (effector (deref strength)))
rlm@404 70 strengths effectors)))
rlm@404 71 :keybindings
rlm@404 72 ;; assume muscles are listed in pairs and map them to keys.
rlm@404 73 (zipmap worm-keys activators)}))
rlm@400 74
rlm@400 75 ;; These are scripts that direct the worm to move in two radically
rlm@400 76 ;; different patterns -- a sinusoidal wiggling motion, and a curling
rlm@400 77 ;; motions that causes the worm to form a circle.
rlm@400 78
rlm@400 79 (def curl-script
rlm@400 80 [[370 :d-up 40]
rlm@400 81 [600 :d-up 0]])
rlm@400 82
rlm@400 83 (def period 18)
rlm@400 84
rlm@404 85 (def worm-muscle-labels
rlm@399 86 [:base-up :base-down
rlm@404 87 :a-down :a-up
rlm@399 88 :b-up :b-down
rlm@404 89 :c-down :c-up
rlm@400 90 :d-up :d-down])
rlm@399 91
rlm@399 92 (defn gen-wiggle [[flexor extensor :as muscle-pair] time-base]
rlm@399 93 (let [period period
rlm@399 94 power 45]
rlm@399 95 [[time-base flexor power]
rlm@399 96 [(+ time-base period) flexor 0]
rlm@399 97 [(+ time-base period 1) extensor power]
rlm@399 98 [(+ time-base (+ (* 2 period) 2)) extensor 0]]))
rlm@399 99
rlm@399 100 (def wiggle-script
rlm@406 101 (mapcat gen-wiggle (repeat 4000 [:a-down :a-up])
rlm@406 102 (range 100 1000000 (+ 3 (* period 2)))))
rlm@399 103
rlm@399 104
rlm@400 105 ;; Normally, we'd use unsupervised/supervised machine learning to pick
rlm@400 106 ;; out the defining features of the different actions available to the
rlm@400 107 ;; worm. For this project, I am going to explicitely define functions
rlm@400 108 ;; that recognize curling and wiggling respectively. These functions
rlm@400 109 ;; are defined using all the information available from an embodied
rlm@400 110 ;; simulation of the action. Note how much easier they are to define
rlm@400 111 ;; than if I only had vision to work with. Things like scale/position
rlm@400 112 ;; invariance are complete non-issues here. This is the advantage of
rlm@400 113 ;; body-centered action recognition and what I hope to show with this
rlm@400 114 ;; thesis.
rlm@400 115
rlm@405 116
rlm@405 117 (defn straight?
rlm@405 118 "Is the worm straight?"
rlm@405 119 [experiences]
rlm@405 120 (every?
rlm@405 121 (fn [[_ _ bend]]
rlm@405 122 (< (Math/sin bend) 0.05))
rlm@405 123 (:proprioception (peek experiences))))
rlm@405 124
rlm@405 125 (defn curled?
rlm@405 126 "Is the worm curled up?"
rlm@405 127 [experiences]
rlm@405 128 (every?
rlm@405 129 (fn [[_ _ bend]]
rlm@405 130 (> (Math/sin bend) 0.64))
rlm@405 131 (:proprioception (peek experiences))))
rlm@405 132
rlm@405 133 (defn grand-circle?
rlm@405 134 "Does the worm form a majestic circle (one end touching the other)?"
rlm@405 135 [experiences]
rlm@405 136 (and (curled? experiences)
rlm@406 137 true)) ;; TODO: add code here.
rlm@405 138
rlm@405 139 (defn vector:last-n [v n]
rlm@405 140 (let [c (count v)]
rlm@405 141 (if (< c n) v
rlm@405 142 (subvec v (- c n) c))))
rlm@400 143
rlm@406 144 (defn touch-average [[coords touch]]
rlm@406 145 (/ (average (map first touch)) (average (map second touch))))
rlm@406 146
rlm@407 147 (def worm-segment-touch-bottom
rlm@407 148 [[8 15] [8 16] [8 17] [8 18] [8 19] [8 20] [8 21] [8 22] [9 15]
rlm@407 149 [9 16] [9 17] [9 18] [9 19] [9 20] [9 21] [9 22] [10 15] [10 16]
rlm@407 150 [10 17] [10 18] [10 19] [10 20] [10 21] [10 22] [11 15] [11 16]
rlm@407 151 [11 17] [11 18] [11 19] [11 20] [11 21] [11 22] [12 15] [12 16]
rlm@407 152 [12 17] [12 18] [12 19] [12 20] [12 21] [12 22] [13 15] [13 16]
rlm@407 153 [13 17] [13 18] [13 19] [13 20] [13 21] [13 22] [14 15] [14 16]
rlm@407 154 [14 17] [14 18] [14 19] [14 20] [14 21] [14 22]])
rlm@407 155
rlm@407 156
rlm@407 157
rlm@406 158 (defn floor-contact [[coords contact :as touch]]
rlm@406 159 (let [raw-average
rlm@406 160 (average
rlm@406 161 (map
rlm@406 162 first
rlm@406 163 (vals
rlm@406 164 (select-keys
rlm@406 165 (zipmap coords contact)
rlm@407 166 ))))]
rlm@406 167 (Math/abs (- 1. (* 10 raw-average)))))
rlm@406 168
rlm@406 169
rlm@400 170 (defn wiggling?
rlm@405 171 "Is the worm wiggling?"
rlm@405 172 [experiences]
rlm@406 173 (vector:last-n experiences 200)
rlm@405 174
rlm@406 175 )
rlm@400 176
rlm@400 177 (def standard-world-view
rlm@400 178 [(Vector3f. 4.207176, -3.7366982, 3.0816958)
rlm@400 179 (Quaternion. 0.11118768, 0.87678415, 0.24434438, -0.3989771)])
rlm@400 180
rlm@400 181 (def worm-side-view
rlm@400 182 [(Vector3f. 4.207176, -3.7366982, 3.0816958)
rlm@400 183 (Quaternion. -0.11555642, 0.88188726, -0.2854942, -0.3569518)])
rlm@400 184
rlm@400 185 (def degenerate-worm-view
rlm@400 186 [(Vector3f. -0.0708936, -8.570261, 2.6487997)
rlm@400 187 (Quaternion. -2.318909E-4, 0.9985348, 0.053941682, 0.004291452)])
rlm@399 188
rlm@404 189 (defn worm-world-defaults []
rlm@404 190 (let [direct-control (worm-direct-control worm-muscle-labels 40)]
rlm@404 191 {:view worm-side-view
rlm@404 192 :motor-control (:motor-control direct-control)
rlm@404 193 :keybindings (:keybindings direct-control)
rlm@405 194 :record nil
rlm@407 195 :experiences nil
rlm@407 196 :worm-model worm-model
rlm@407 197 :end-frame nil}))
rlm@407 198
rlm@404 199
rlm@404 200 (defn dir! [file]
rlm@404 201 (if (not (.exists file))
rlm@404 202 (.mkdir file))
rlm@404 203 file)
rlm@405 204
rlm@405 205 (defn record-experience! [experiences data]
rlm@405 206 (swap! experiences #(conj % data)))
rlm@405 207
rlm@399 208 (defn worm-world
rlm@407 209 [& {:keys [record motor-control keybindings view experiences
rlm@407 210 worm-model end-frame] :as settings}]
rlm@407 211 (let [{:keys [record motor-control keybindings view experiences
rlm@407 212 worm-model end-frame]}
rlm@404 213 (merge (worm-world-defaults) settings)
rlm@404 214 worm (doto (worm-model) (body!))
rlm@404 215 touch (touch! worm)
rlm@404 216 prop (proprioception! worm)
rlm@404 217 muscles (movement! worm)
rlm@404 218
rlm@404 219 touch-display (view-touch)
rlm@404 220 prop-display (view-proprioception)
rlm@404 221 muscle-display (view-movement)
rlm@404 222
rlm@404 223 floor (box 10 1 10 :position (Vector3f. 0 -10 0)
rlm@407 224 :color ColorRGBA/Gray :mass 0)
rlm@407 225 timer (IsoTimer. 60)]
rlm@399 226
rlm@404 227 (world
rlm@404 228 (nodify [worm floor])
rlm@404 229 (merge standard-debug-controls keybindings)
rlm@404 230 (fn [world]
rlm@404 231 (position-camera world view)
rlm@407 232 (.setTimer world timer)
rlm@407 233 (display-dilated-time world timer)
rlm@404 234 (if record
rlm@404 235 (Capture/captureVideo
rlm@404 236 world
rlm@404 237 (dir! (File. record "main-view"))))
rlm@404 238 (speed-up world)
rlm@404 239 (light-up-everything world))
rlm@404 240 (fn [world tpf]
rlm@407 241 (if (> (.getTime timer) end-frame)
rlm@407 242 (.stop world))
rlm@405 243 (let [muscle-data (motor-control muscles)
rlm@405 244 proprioception-data (prop)
rlm@405 245 touch-data (map #(% (.getRootNode world)) touch)]
rlm@405 246 (when experiences
rlm@405 247 (record-experience!
rlm@405 248 experiences {:touch touch-data
rlm@405 249 :proprioception proprioception-data
rlm@405 250 :muscle muscle-data})
rlm@408 251 ;;(if (curled? @experiences) (println "Curled"))
rlm@406 252 ;;(if (straight? @experiences) (println "Straight"))
rlm@407 253 ;; (println-repl
rlm@407 254 ;; (apply format "%.2f %.2f %.2f %.2f %.2f\n"
rlm@407 255 ;; (map floor-contact touch-data)))
rlm@407 256
rlm@405 257 )
rlm@404 258 (muscle-display
rlm@405 259 muscle-data
rlm@405 260 (if record (dir! (File. record "muscle"))))
rlm@405 261 (prop-display
rlm@405 262 proprioception-data
rlm@405 263 (if record (dir! (File. record "proprio"))))
rlm@405 264 (touch-display
rlm@405 265 touch-data
rlm@405 266 (if record (dir! (File. record "touch")))))))))
rlm@407 267
rlm@407 268
rlm@407 269 ;; A demonstration of self organiging touch maps through experience.
rlm@407 270
rlm@407 271 (def single-worm-segment-view
rlm@407 272 [(Vector3f. 2.0681207, -6.1406755, 1.6106138)
rlm@407 273 (Quaternion. -0.15558705, 0.843615, -0.3428654, -0.38281822)])
rlm@407 274
rlm@407 275 (def worm-single-segment-muscle-labels
rlm@407 276 [:lift-1 :lift-2 :roll-1 :roll-2])
rlm@407 277
rlm@407 278 (defn touch-kinesthetics []
rlm@407 279 [[170 :lift-1 40]
rlm@407 280 [190 :lift-1 20]
rlm@407 281 [206 :lift-1 0]
rlm@407 282
rlm@407 283 [400 :lift-2 40]
rlm@407 284 [410 :lift-2 0]
rlm@407 285
rlm@407 286 [570 :lift-2 40]
rlm@407 287 [590 :lift-2 20]
rlm@407 288 [606 :lift-2 0]
rlm@407 289
rlm@408 290 [800 :lift-1 30]
rlm@407 291 [809 :lift-1 0]
rlm@407 292
rlm@407 293 [900 :roll-2 40]
rlm@407 294 [905 :roll-2 20]
rlm@407 295 [910 :roll-2 0]
rlm@407 296
rlm@407 297 [1000 :roll-2 40]
rlm@407 298 [1005 :roll-2 20]
rlm@407 299 [1010 :roll-2 0]
rlm@407 300
rlm@407 301 [1100 :roll-2 40]
rlm@407 302 [1105 :roll-2 20]
rlm@407 303 [1110 :roll-2 0]
rlm@407 304 ])
rlm@407 305
rlm@408 306 (defn single-worm-segment []
rlm@408 307 (load-blender-model "Models/worm/worm-single-segment.blend"))
rlm@408 308
rlm@407 309 (defn worm-segment-defaults []
rlm@407 310 (let [direct-control (worm-direct-control worm-muscle-labels 40)]
rlm@407 311 (merge (worm-world-defaults)
rlm@407 312 {:worm-model single-worm-segment
rlm@407 313 :view single-worm-segment-view
rlm@407 314 :motor-control
rlm@407 315 (motor-control-program
rlm@407 316 worm-single-segment-muscle-labels
rlm@408 317 (touch-kinesthetics))
rlm@408 318 :end-frame 1200})))
rlm@407 319
rlm@408 320 (def full-contact [(float 0.0) (float 0.1)])
rlm@407 321
rlm@407 322 (defn pure-touch?
rlm@407 323 "This is worm specific code to determine if a large region of touch
rlm@407 324 sensors is either all on or all off."
rlm@407 325 [[coords touch :as touch-data]]
rlm@408 326 (= (set (map first touch)) (set full-contact)))
rlm@408 327
rlm@408 328 (defn remove-similar
rlm@408 329 [coll]
rlm@408 330 (loop [result () coll (sort-by (comp - count) coll)]
rlm@408 331 (if (empty? coll) result
rlm@408 332 (let [x (first coll)
rlm@408 333 xs (rest coll)
rlm@408 334 c (count x)]
rlm@408 335 (if (some
rlm@408 336 (fn [other-set]
rlm@408 337 (let [oc (count other-set)]
rlm@408 338 (< (- (count (union other-set x)) c) (* oc 0.1))))
rlm@408 339 xs)
rlm@408 340 (recur result xs)
rlm@408 341 (recur (cons x result) xs))))))
rlm@408 342
rlm@408 343
rlm@408 344 (defn rect-region [[x0 y0] [x1 y1]]
rlm@408 345 (vec
rlm@408 346 (for [x (range x0 (inc x1))
rlm@408 347 y (range y0 (inc y1))]
rlm@408 348 [x y])))
rlm@408 349
rlm@408 350 (def all-touch-coordinates
rlm@408 351 (concat
rlm@408 352 (rect-region [0 15] [7 22])
rlm@408 353 (rect-region [8 0] [14 29])
rlm@408 354 (rect-region [15 15] [22 22])))
rlm@408 355
rlm@408 356 (defn view-touch-region [coords]
rlm@408 357 (let [touched-region
rlm@408 358 (reduce
rlm@408 359 (fn [m k]
rlm@408 360 (assoc m k [0.0 0.1]))
rlm@408 361 (zipmap all-touch-coordinates (repeat [0.1 0.1])) coords)
rlm@408 362 data
rlm@408 363 [[(vec (keys touched-region)) (vec (vals touched-region))]]
rlm@408 364 touch-display (view-touch)]
rlm@408 365 (touch-display data)
rlm@408 366 (touch-display data)))
rlm@408 367
rlm@408 368 (defn learn-touch-regions []
rlm@408 369 (let [experiences (atom [])
rlm@408 370 world (apply-map
rlm@408 371 worm-world
rlm@408 372 (assoc (worm-segment-defaults)
rlm@408 373 :experiences experiences))]
rlm@408 374 (run-world world)
rlm@408 375 (->>
rlm@408 376 @experiences
rlm@408 377 (drop 175)
rlm@408 378 ;; access the single segment's touch data
rlm@408 379 (map (comp first :touch))
rlm@408 380 ;; only deal with "pure" touch data to determine surfaces
rlm@408 381 (filter pure-touch?)
rlm@408 382 ;; associate coordinates with touch values
rlm@408 383 (map (partial apply zipmap))
rlm@408 384 ;; select those regions where contact is being made
rlm@408 385 (map (partial group-by second))
rlm@408 386 (map #(get % full-contact))
rlm@408 387 (map (partial map first))
rlm@408 388 ;; remove redundant/subset regions
rlm@408 389 (map set)
rlm@408 390 remove-similar)))
rlm@408 391
rlm@408 392 (defn learn-and-view-touch-regions []
rlm@408 393 (map view-touch-region
rlm@408 394 (learn-touch-regions)))
rlm@408 395