view org/worm_learn.clj @ 430:5205535237fb

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
line wrap: on
line source
1 (ns org.aurellem.worm-learn
2 "General worm creation framework."
3 {:author "Robert McIntyre"}
4 (:use (cortex world util import body sense
5 hearing touch vision proprioception movement
6 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-program
34 "Create a function which will execute the motor script"
35 [muscle-labels
36 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 (dorun
43 (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 effectors
48 (map #(@current-forces % 0) muscle-labels)))))))
50 (defn worm-direct-control
51 "Create keybindings and a muscle control program that will enable
52 the user to control the worm via the keyboard."
53 [muscle-labels activation-strength]
54 (let [strengths (mapv (fn [_] (atom 0)) muscle-labels)
55 activator
56 (fn [n]
57 (fn [world pressed?]
58 (let [strength (if pressed? activation-strength 0)]
59 (swap! (nth strengths n) (constantly strength)))))
60 activators
61 (map activator (range (count muscle-labels)))
62 worm-keys
63 ["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-control
70 (fn [effectors]
71 (doall
72 (map (fn [strength effector]
73 (effector (deref strength)))
74 strengths effectors)))
75 :keybindings
76 ;; 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 radically
80 ;; different patterns -- a sinusoidal wiggling motion, and a curling
81 ;; motions that causes the worm to form a circle.
83 (def curl-script
84 [[150 :d-flex 40]
85 [250 :d-flex 0]])
87 (def period 18)
89 (def worm-muscle-labels
90 [:base-ex :base-flex
91 :a-ex :a-flex
92 :b-ex :b-flex
93 :c-ex :c-flex
94 :d-ex :d-flex])
96 (defn gen-wiggle [[flexor extensor :as muscle-pair] time-base]
97 (let [period period
98 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-script
105 (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-things
114 (concat
115 curl-script
116 [[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 pick
121 ;; out the defining features of the different actions available to the
122 ;; worm. For this project, I am going to explicitely define functions
123 ;; that recognize curling and wiggling respectively. These functions
124 ;; are defined using all the information available from an embodied
125 ;; simulation of the action. Note how much easier they are to define
126 ;; than if I only had vision to work with. Things like scale/position
127 ;; invariance are complete non-issues here. This is the advantage of
128 ;; body-centered action recognition and what I hope to show with this
129 ;; thesis.
132 ;; curled? relies on proprioception, resting? relies on touch,
133 ;; wiggling? relies on a fourier analysis of muscle contraction, and
134 ;; 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 (vec
146 (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 contact
153 "Determine how much contact a particular worm segment has with
154 other objects. Returns a value between 0 and 1, where 1 is full
155 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 straight?"
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) v
177 (subvec v (- c n) c))))
179 (defn fft [nums]
180 (map
181 #(.getReal %)
182 (.transform
183 (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 3
197 a-ex 2
198 muscle-activity
199 (map :muscle (vector:last-n experiences analysis-interval))
200 base-activity
201 (map #(- (% a-flex) (% a-ex)) muscle-activity)]
202 (= 2
203 (first
204 (max-indexed
205 (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-experience
226 [experiences]
227 (cond
228 (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-view
235 [(Vector3f. 4.207176, -3.7366982, 3.0816958)
236 (Quaternion. 0.11118768, 0.87678415, 0.24434438, -0.3989771)])
238 (def worm-side-view
239 [(Vector3f. 4.207176, -3.7366982, 3.0816958)
240 (Quaternion. -0.11555642, 0.88188726, -0.2854942, -0.3569518)])
242 (def degenerate-worm-view
243 [(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-control
249 {:view worm-side-view
250 :record nil
251 :experiences (atom [])
252 :experience-watch debug-experience
253 :worm-model worm-model
254 :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)))
264 (defn worm-world
265 [& {:keys [record motor-control keybindings view experiences
266 worm-model end-frame experience-watch] :as settings}]
267 (let [{:keys [record motor-control keybindings view experiences
268 worm-model end-frame experience-watch]}
269 (merge (worm-world-defaults) settings)
270 worm (doto (worm-model) (body!))
271 touch (touch! worm)
272 prop (proprioception! worm)
273 muscles (movement! worm)
275 touch-display (view-touch)
276 prop-display (view-proprioception)
277 muscle-display (view-movement)
279 floor (box 10 1 10 :position (Vector3f. 0 -10 0)
280 :color ColorRGBA/Gray :mass 0)
281 timer (IsoTimer. 60)]
283 (world
284 (nodify [worm floor])
285 (merge standard-debug-controls keybindings)
286 (fn [world]
287 (position-camera world view)
288 (.setTimer world timer)
289 (display-dilated-time world timer)
290 (when record
291 (Capture/captureVideo
292 world
293 (dir! (File. record "main-view"))))
294 (speed-up world)
295 (light-up-everything world))
296 (fn [world tpf]
297 (if (and end-frame (> (.getTime timer) end-frame))
298 (.stop world))
299 (let [muscle-data (vec (motor-control muscles))
300 proprioception-data (prop)
301 touch-data (mapv #(% (.getRootNode world)) touch)]
302 (when experiences
303 (record-experience!
304 experiences {:touch touch-data
305 :proprioception proprioception-data
306 :muscle muscle-data}))
307 (when experience-watch
308 (experience-watch @experiences))
309 (muscle-display
310 muscle-data
311 (when record (dir! (File. record "muscle"))))
312 (prop-display
313 proprioception-data
314 (when record (dir! (File. record "proprio"))))
315 (touch-display
316 touch-data
317 (when record (dir! (File. record "touch")))))))))
321 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
322 ;;;;;;;; Phi-Space ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
323 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
325 (defn generate-phi-space []
326 (let [experiences (atom [])]
327 (run-world
328 (apply-map
329 worm-world
330 (merge
331 (worm-world-defaults)
332 {:end-frame 700
333 :motor-control
334 (motor-control-program worm-muscle-labels do-all-the-things)
335 :experiences experiences})))
336 @experiences))
338 (defn bin [digits]
339 (fn [angles]
340 (->> angles
341 (flatten)
342 (map (juxt #(Math/sin %) #(Math/cos %)))
343 (flatten)
344 (mapv #(Math/round (* % (Math/pow 10 (dec digits))))))))
346 ;; k-nearest neighbors with spatial binning. Only returns a result if
347 ;; the propriceptive data is within 10% of a previously recorded
348 ;; result in all dimensions.
349 (defn gen-phi-scan [phi-space]
350 (let [bin-keys (map bin [3 2 1])
351 bin-maps
352 (map (fn [bin-key]
353 (group-by
354 (comp bin-key :proprioception phi-space)
355 (range (count phi-space)))) bin-keys)
356 lookups (map (fn [bin-key bin-map]
357 (fn [proprio] (bin-map (bin-key proprio))))
358 bin-keys bin-maps)]
359 (fn lookup [proprio-data]
360 (set (some #(% proprio-data) lookups)))))
363 (defn longest-thread
364 "Find the longest thread from phi-index-sets. The index sets should
365 be ordered from most recent to least recent."
366 [phi-index-sets]
367 (loop [result '()
368 [thread-bases & remaining :as phi-index-sets] phi-index-sets]
369 (if (empty? phi-index-sets)
370 (vec result)
371 (let [threads
372 (for [thread-base thread-bases]
373 (loop [thread (list thread-base)
374 remaining remaining]
375 (let [next-index (dec (first thread))]
376 (cond (empty? remaining) thread
377 (contains? (first remaining) next-index)
378 (recur
379 (cons next-index thread) (rest remaining))
380 :else thread))))
381 longest-thread
382 (reduce (fn [thread-a thread-b]
383 (if (> (count thread-a) (count thread-b))
384 thread-a thread-b))
385 '(nil)
386 threads)]
387 (recur (concat longest-thread result)
388 (drop (count longest-thread) phi-index-sets))))))
391 (defn init []
392 (def phi-space (generate-phi-space))
393 (def phi-scan (gen-phi-scan phi-space))
394 )
396 ;; (defn infer-nils-dyl [s]
397 ;; (loop [closed ()
398 ;; open s
399 ;; anchor 0]
400 ;; (if-not (empty? open)
401 ;; (recur (conj closed
402 ;; (or (peek open)
403 ;; anchor))
404 ;; (pop open)
405 ;; (or (peek open) anchor))
406 ;; closed)))
408 ;; (defn infer-nils [s]
409 ;; (for [i (range (count s))]
410 ;; (or (get s i)
411 ;; (some (comp not nil?) (vector:last-n (- (count s) i)))
412 ;; 0)))
415 (defn infer-nils
416 "Replace nils with the next available non-nil element in the
417 sequence, or barring that, 0."
418 [s]
419 (loop [i (dec (count s))
420 v (transient s)]
421 (if (zero? i) (persistent! v)
422 (if-let [cur (v i)]
423 (if (get v (dec i) 0)
424 (recur (dec i) v)
425 (recur (dec i) (assoc! v (dec i) cur)))
426 (recur i (assoc! v i 0))))))
428 ;; tests
430 ;;(infer-nils [1 nil 1 1]) [1 1 1 1]
431 ;;(infer-nils [1 1 1 nil]) [1 1 1 0]
432 ;;(infer-nils [nil 2 1 1]) [2 2 1 1]
435 (defn debug-experience-phi []
436 (let [proprio (atom ())]
437 (fn
438 [experiences]
439 (let [phi-indices (phi-scan (:proprioception (peek experiences)))]
440 (swap! proprio (partial cons phi-indices))
441 (let [exp-thread (longest-thread (take 300 @proprio))
442 phi-loop (mapv phi-space (infer-nils exp-thread))]
443 (println-repl (vector:last-n exp-thread 22))
444 (cond
445 (grand-circle? phi-loop) (println "Grand Circle")
446 (curled? phi-loop) (println "Curled")
447 (wiggling? phi-loop) (println "Wiggling")
448 (resting? phi-loop) (println "Resting")
449 :else (println "Unknown")))))))
452 (defn init-interactive []
453 (def phi-space
454 (let [experiences (atom [])]
455 (run-world
456 (apply-map
457 worm-world
458 (merge
459 (worm-world-defaults)
460 {:experiences experiences})))
461 @experiences))
462 (def phi-scan (gen-phi-scan phi-space)))
465 (defn run-experiment-1 []
466 (.start (worm-world :experience-watch (debug-experience-phi))))