Mercurial > cortex

annotate org/worm_learn.clj @ 408:3b4012b42611

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
completed demonstration showing automatic partitioning of touch space based on experience.
author Robert McIntyre <rlm@mit.edu>
date Tue, 18 Mar 2014 21:28:04 -0400
parents bd6d03596ea8
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