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
|