Mercurial > cortex

comparison org/body.org @ 63:7f2653ad3199

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
cleaning
author Robert McIntyre <rlm@mit.edu>
date Tue, 29 Nov 2011 02:46:46 -0700
parents 2b9d81017cb7
children ab1fee4280c3
comparison
equal deleted inserted replaced
62:2b9d81017cb7 63:7f2653ad3199
3 #+email: rlm@mit.edu 3 #+email: rlm@mit.edu
4 #+description: Simulating a body (movement, touch, propioception) in jMonkeyEngine3. 4 #+description: Simulating a body (movement, touch, propioception) in jMonkeyEngine3.
5 #+SETUPFILE: ../../aurellem/org/setup.org 5 #+SETUPFILE: ../../aurellem/org/setup.org
6 #+INCLUDE: ../../aurellem/org/level-0.org 6 #+INCLUDE: ../../aurellem/org/level-0.org
7 7
8 * Body 8 * COMMENT Body
9 9
10 #+srcname: body-main 10 #+srcname: body-main
11 #+begin_src clojure 11 #+begin_src clojure
12 (ns cortex.body 12 (ns cortex.body
13 (use (cortex world util import))) 13 (use (cortex world util import)))
14 14
15 (use 'clojure.contrib.def) 15 (use 'clojure.contrib.def)
16 (cortex.import/mega-import-jme3) 16 (cortex.import/mega-import-jme3)
17 (rlm.rlm-commands/help) 17 (rlm.rlm-commands/help)
18 18
19 (defn load-blender-model 19 (defn load-blender-model
20 "Load a .blend file using an asset folder relative path."
20 [^String model] 21 [^String model]
21 (.loadModel 22 (.loadModel
22 (doto (asset-manager) 23 (doto (asset-manager)
23 (.registerLoader BlenderModelLoader (into-array String ["blend"]))) 24 (.registerLoader BlenderModelLoader (into-array String ["blend"])))
24 model)) 25 model))
25 26
26 (defn skel [node]
27 (doto
28 (.getSkeleton
29 (.getControl node SkeletonControl))
30 ;; this is necessary to force the skeleton to have accurate world
31 ;; transforms before it is rendered to the screen.
32 (.resetAndUpdate)))
33
34
35 (defn green-x-ray []
36 (doto (Material. (asset-manager)
37 "Common/MatDefs/Misc/Unshaded.j3md")
38 (.setColor "Color" ColorRGBA/Green)
39 (-> (.getAdditionalRenderState)
40 (.setDepthTest false))))
41
42
43 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 27 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
44 28
45 ;;;;;;;;;;;; eve-style bodies ;;;;;;;; 29 ;;;;;;;;;;;; eve-style bodies ;;;;;;;;
46 30
47 (defn worm [segment-length num-segments interstitial-space radius] 31 (defn worm-segments
32 "Create multiple evenly spaced box segments. They're fabulous!"
33 [segment-length num-segments interstitial-space radius]
48 (letfn [(nth-segment 34 (letfn [(nth-segment
49 [n] 35 [n]
50 (box segment-length radius radius :mass 0.1 36 (box segment-length radius radius :mass 0.1
51 :position 37 :position
52 (Vector3f. 38 (Vector3f.
53 (* 2 n (+ interstitial-space segment-length)) 0 0) 39 (* 2 n (+ interstitial-space segment-length)) 0 0)
54 :name (str "worm-segment" n) 40 :name (str "worm-segment" n)
55 :color (ColorRGBA/randomColor)))] 41 :color (ColorRGBA/randomColor)))]
56 (map nth-segment (range num-segments)))) 42 (map nth-segment (range num-segments))))
57 43
58
59 (defn connect-at-midpoint 44 (defn connect-at-midpoint
45 "Connect two physics objects with a Point2Point joint constraint at
46 the point equidistant from both objects' centers."
60 [segmentA segmentB] 47 [segmentA segmentB]
61 (let [centerA (.getWorldTranslation segmentA) 48 (let [centerA (.getWorldTranslation segmentA)
62 centerB (.getWorldTranslation segmentB) 49 centerB (.getWorldTranslation segmentB)
63 midpoint (.mult (.add centerA centerB) (float 0.5)) 50 midpoint (.mult (.add centerA centerB) (float 0.5))
64 pivotA (.subtract midpoint centerA) 51 pivotA (.subtract midpoint centerA)
65 pivotB (.subtract midpoint centerB) 52 pivotB (.subtract midpoint centerB)
53
54 ;; A side-effect of creating a joint registers
55 ;; it with both physics objects which in turn
56 ;; will register the joint with the physics system
57 ;; when the simulation is started.
66 joint (Point2PointJoint. 58 joint (Point2PointJoint.
67 (.getControl segmentA RigidBodyControl) 59 (.getControl segmentA RigidBodyControl)
68 (.getControl segmentB RigidBodyControl) 60 (.getControl segmentB RigidBodyControl)
69 pivotA 61 pivotA
70 pivotB)] 62 pivotB)]
71 segmentB)) 63 segmentB))
72 64
73 (defn point-worm [] 65 (defn point-worm []
74 (let [segments (worm 0.2 5 0.1 0.1)] 66 (let [segments (worm-segments 0.2 5 0.1 0.1)]
75 (dorun (map (partial apply connect-at-midpoint) 67 (dorun (map (partial apply connect-at-midpoint)
76 (partition 2 1 segments))) 68 (partition 2 1 segments)))
77 (nodify "worm" segments))) 69 (nodify "worm" segments)))
78 70
79 (defn test-worm [] 71 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
80 (.start 72 ;;;;;;;;; Proprioception ;;;;;;;;;;;;;
81 (world 73 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
82 (doto (Node.) 74
83 ;;(.attachChild (point-worm)) 75 (declare
84 (.attachChild (load-blender-model 76 ;; generate an arbitray (but stable) orthogonal vector
85 "Models/anim2/joint-worm.blend")) 77 ;; to a given vector.
86 78 some-orthogonal
87 (.attachChild (box 10 1 10 79
88 :position (Vector3f. 0 -2 0) :mass 0 80 ;; determine the amount of rotation a quaternion will
89 :color (ColorRGBA/Gray)))) 81 ;; cause about a given axis
90 { 82 project-quaternion
91 "key-space" (fire-cannon-ball) 83
92 } 84 ;; proprioception for a single joint
93 (fn [world] 85 joint-proprioception
94 (enable-debug world) 86
95 (light-up-everything world) 87 ;; create a function that provides proprioceptive information
96 ;;(.setTimer world (NanoTimer.)) 88 ;; about an entire body.
97 ) 89 proprioception)
98 no-op))) 90
99 91 (defn some-orthogonal
100 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 92 "Generate an arbitray (but stable) orthogonal vector to a given
101 93 vector."
102
103 ;;;;;;;;; Mortor Control ;;;;;;;;;;;;;
104
105
106 ;; surprisingly ehough, terristerial creatures only move by using
107 ;; torque applied to their joints. There's not a single straight line
108 ;; of force in the human body at all! (a straight line of force would
109 ;; correspond to some sort of jet or rocket propulseion)
110
111 (defn torque-controls [control]
112 (let [torques
113 (concat
114 (map #(Vector3f. 0 (Math/sin %) (Math/cos %))
115 (range 0 (* Math/PI 2) (/ (* Math/PI 2) 20)))
116 [Vector3f/UNIT_X])]
117 (map (fn [torque-axis]
118 (fn [torque]
119 (.applyTorque
120 control
121 (.mult (.mult (.getPhysicsRotation control)
122 torque-axis)
123 (float
124 (* (.getMass control) torque))))))
125 torques)))
126
127 (defn motor-map
128 "Take a creature and generate a function that will enable fine
129 grained control over all the creature's limbs."
130 [#^Node creature]
131 (let [controls (keep #(.getControl % RigidBodyControl)
132 (node-seq creature))
133 limb-controls (reduce concat (map torque-controls controls))
134 body-control (partial map #(%1 %2) limb-controls)]
135 body-control))
136
137 (defn test-motor-map
138 "see how torque works."
139 []
140 (let [finger (box 3 0.5 0.5 :position (Vector3f. 0 2 0)
141 :mass 1 :color ColorRGBA/Green)
142 motor-map (motor-map finger)]
143 (world
144 (nodify [finger
145 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
146 :color ColorRGBA/Gray)])
147 standard-debug-controls
148 (fn [world]
149 (set-gravity world Vector3f/ZERO)
150 (light-up-everything world)
151 (.setTimer world (NanoTimer.)))
152 (fn [_ _]
153 (dorun (motor-map [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0]))))))
154
155 (defn test-torque
156 "see how torque works."
157 []
158 (let [finger (box 3 0.5 0.5 :position (Vector3f. 0 2 0)
159 :mass 1 :color ColorRGBA/Green)
160 move-left? (atom false)
161 move-right? (atom false)
162 control (.getControl finger RigidBodyControl)]
163 (world
164 (nodify [finger
165 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
166 :color ColorRGBA/Gray)])
167 (merge standard-debug-controls
168 {"key-k" (fn [_ pressed?] (reset! move-left? pressed?))
169 "key-l" (fn [_ pressed?] (reset! move-right? pressed?))})
170 (fn [world]
171 (set-gravity world Vector3f/ZERO)
172 (light-up-everything world)
173 (.setTimer world (NanoTimer.)))
174 (fn [_ _]
175 (if @move-left?
176 (.applyTorque control
177 (.mult (.getPhysicsRotation control)
178 (Vector3f. -3 20 0))))
179 (if @move-right?
180 (.applyTorque control (Vector3f. 0 0 1)))))))
181
182 (defn worm-pattern [time]
183 [ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
184
185 0 0 0 0 0 0 0 0 0 0 0
186
187 (* 20 (Math/sin (* Math/PI 2 (/ (rem time 300 ) 300))))
188
189 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
190 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
191 0 0 0 0 0 0 0 0 0 0 0 0 0 0
192
193 ])
194
195 ;;;;;;;;;;;;;;;;;; Proprioception ;;;;;;;;;;;;;;;;;;;;;;;;
196
197 ;; this is not used as just getting the rotation would be simpler.
198 (defn proprioception-senses
199 "given a control , create a sequence of thunks that will report the
200 rotation of the control's object along the same axes as the motor-control map."
201 [control]
202 (let [torques
203 (concat
204 (map #(Vector3f. 0 (Math/sin %) (Math/cos %))
205 (range 0 (* Math/PI 2) (/ (* Math/PI 2) 20)))
206 [Vector3f/UNIT_X])]
207 (map (fn [torque-axis]
208 (fn []
209 (.getPhysicsRotation control)))
210 torques)))
211
212 (defn orthogonal-vect
213 "Return a vector orthogonal to the current one"
214 [vector] 94 [vector]
215 (let [x (.getX vector) 95 (let [x (.getX vector)
216 y (.getY vector) 96 y (.getY vector)
217 z (.getZ vector)] 97 z (.getZ vector)]
218 (cond 98 (cond
219 (not= x (float 0)) (Vector3f. (- z) 0 x) 99 (not= x (float 0)) (Vector3f. (- z) 0 x)
220 (not= y (float 0)) (Vector3f. 0 (- z) y) 100 (not= y (float 0)) (Vector3f. 0 (- z) y)
221 (not= z (float 0)) (Vector3f. 0 (- z) y) 101 (not= z (float 0)) (Vector3f. 0 (- z) y)
222 true Vector3f/ZERO))) 102 true Vector3f/ZERO)))
223 103
224 ;; from 104 (defn project-quaternion
225 ;; http://stackoverflow.com/questions/3684269/ \\ 105 "From http://stackoverflow.com/questions/3684269/
226 ;; component-of-a-quaternion-rotation-around-an-axis 106 component-of-a-quaternion-rotation-around-an-axis.
227 (defn rot-about-axis [#^Quaternion q #^Vector3f axis] 107
108 Determine the amount of rotation a quaternion will
109 cause about a given axis."
110 [#^Quaternion q #^Vector3f axis]
228 (let [basis-1 (orthogonal-vect axis) 111 (let [basis-1 (orthogonal-vect axis)
229 basis-2 (.cross axis basis-1) 112 basis-2 (.cross axis basis-1)
230 rotated (.mult q basis-1) 113 rotated (.mult q basis-1)
231 alpha (.dot basis-1 (.project rotated basis-1)) 114 alpha (.dot basis-1 (.project rotated basis-1))
232 beta (.dot basis-2 (.project rotated basis-2))] 115 beta (.dot basis-2 (.project rotated basis-2))]
233 (Math/atan2 beta alpha))) 116 (Math/atan2 beta alpha)))
234 117
235 118 (defn joint-proprioception
236 (defn check-rot [a] 119 "Relative position information for a two-part system connected by a
237 (rot-about-axis 120 joint. Gives the pitch, yaw, and roll of the 'B' object relative to
238 (doto (Quaternion.) 121 the 'A' object, as determined by the joint."
239 (.fromAngleAxis 122 [joint]
240 (float a)
241 (Vector3f. 1 0 0))) (Vector3f. 1 0 0)))
242
243 (defn relative-positions [joint]
244 (let [object-a (.getUserObject (.getBodyA joint)) 123 (let [object-a (.getUserObject (.getBodyA joint))
245 object-b (.getUserObject (.getBodyB joint)) 124 object-b (.getUserObject (.getBodyB joint))
246 arm-a 125 arm-a
247 (.normalize 126 (.normalize
248 (.subtract 127 (.subtract
271 (rot-about-axis 150 (rot-about-axis
272 (.mult 151 (.mult
273 (.getLocalRotation object-b) 152 (.getLocalRotation object-b)
274 (doto (Quaternion.) 153 (doto (Quaternion.)
275 (.fromRotationMatrix rotate-a))) 154 (.fromRotationMatrix rotate-a)))
276 arm-b) 155 arm-b)]
277 ]
278
279
280
281 ;;(println-repl
282 ;; "arm-b is " arm-b)
283 ;;(println-repl
284 ;; "pivot-b is " (.getPivotB joint))
285 ;;(println-repl
286 ;; (format "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n"
287 ;; pitch yaw roll))
288 [pitch yaw roll])) 156 [pitch yaw roll]))
289 157
290 158 (defn proprioception
291 159 "Create a function that provides proprioceptive information about an
292 160 entire body."
161 [body]
162 ;; extract the body's joints
163 (let [joints
164 (distinct
165 (reduce
166 concat
167 (map #(.getJoints %)
168 (keep
169 #(.getControl % RigidBodyControl)
170 (node-seq body)))))]
171 (fn []
172 (map joint-proprioception joints))))
293 173
294 174
175 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
176 ;;;;;;;;; Mortor Control ;;;;;;;;;;;;;
177 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
178
179
180 ;; surprisingly enough, terristerial creatures only move by using
181 ;; torque applied about their joints. There's not a single straight
182 ;; line of force in the human body at all! (A straight line of force
183 ;; would correspond to some sort of jet or rocket propulseion.)
184
185 (defn vector-motor-control
186 "Create a function that accepts a sequence of Vector3f objects that
187 describe the torque to be applied to each part of the body."
188 [body]
189 (let [nodes (node-seq body)
190 controls (keep #(.getControl % RigidBodyControl) nodes)]
191 (fn [torques]
192 (map #(.applyTorque %1 %2)
193 controls torques))))
194
195 ;; note -- might want to add a lower dimensional, discrete version of
196 ;; this if it proves usefull from a x-modal clustering perspective.
197
198 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
199
200
201
202 (defn worm-pattern [time]
203 [ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
204
205 0 0 0 0 0 0 0 0 0 0 0
206
207 (* 20 (Math/sin (* Math/PI 2 (/ (rem time 300 ) 300))))
208
209 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
210 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
211 0 0 0 0 0 0 0 0 0 0 0 0 0 0
212
213 ])
214
215 (defn worm-pattern [time]
216 (let [angle (* Math/PI (/ 4 20))
217 direction (Vector3f. 0 (Math/sin angle) (Math/cos angle))]
218 [Vector3f/ZERO
219 (.mult
220 direction
221 (float (* 2 (Math/sin (* Math/PI 2 (/ (rem time 300 ) 300))))))
222 Vector3f/ZERO
223 Vector3f/ZERO
224 Vector3f/ZERO]))
295 225
296 (defn test-worm-control 226 (defn test-worm-control
297 [] 227 []
298 (let [worm (point-worm) 228 (let [worm (point-worm)
299 time (atom 0) 229 time (atom 0)
300 worm-motor-map (motor-map worm) 230 worm-motor-map (vector-motor-control worm)]
301 ;;body-map (proprioception worm)
302 debug-segments
303 (map
304 #(doto
305 (make-shape
306 (assoc base-shape
307 :name "debug-line"
308 :physical? false
309 :shape
310 (com.jme3.scene.shape.Line.
311 (.add (.getWorldTranslation %)
312 (Vector3f. -0.2 0 0 ))
313 (.add (.getWorldTranslation %)
314 (Vector3f. 0.2 0 0)))))
315 (.setMaterial (green-x-ray)))
316 (drop 1 (node-seq worm)))]
317 (world 231 (world
318 (nodify [worm 232 (nodify [worm
319 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0 233 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
320 :color ColorRGBA/Gray)]) 234 :color ColorRGBA/Gray)])
321 standard-debug-controls 235 standard-debug-controls
322 (fn [world] 236 (fn [world]
323 (.attachChild (.getRootNode world) (nodify debug-segments))
324 (enable-debug world) 237 (enable-debug world)
325 (light-up-everything world) 238 (light-up-everything world)
326 (com.aurellem.capture.Capture/captureVideo 239 (comment
327 world 240 (com.aurellem.capture.Capture/captureVideo
328 (file-str "/home/r/proj/cortex/tmp/moving-worm"))) 241 world
242 (file-str "/home/r/proj/cortex/tmp/moving-worm")))
243 )
329 244
330 (fn [_ _] 245 (fn [_ _]
331 (dorun
332 (map
333 (fn [worm-segment
334 debug-segment]
335 (.rotate
336 debug-segment
337 (Quaternion. (float 0) (float 0.05) (float 0) (float 1))))
338 (drop 1 (node-seq worm))
339 debug-segments))
340 (swap! time inc) 246 (swap! time inc)
341 ;;(println-repl (with-out-str (clojure.pprint/pprint (doall (body-map))))) 247 ;;(Thread/sleep 200)
342 (Thread/sleep 200)
343 (dorun (worm-motor-map 248 (dorun (worm-motor-map
344 (worm-pattern @time))))))) 249 (worm-pattern @time)))))))
345
346 250
347 251
348 252
349 253
350 (defn test-prop 254 (defn test-prop
426 (relative-positions joint)))))))) 330 (relative-positions joint))))))))
427 331
428 #+end_src 332 #+end_src
429 333
430 334
431 * COMMENT failed-clojure-code 335 * COMMENT code-limbo
432 #+begin_src clojure 336 #+begin_src clojure
433 ;;(.loadModel 337 ;;(.loadModel
434 ;; (doto (asset-manager) 338 ;; (doto (asset-manager)
435 ;; (.registerLoader BlenderModelLoader (into-array String ["blend"]))) 339 ;; (.registerLoader BlenderModelLoader (into-array String ["blend"])))
436 ;; "Models/person/person.blend") 340 ;; "Models/person/person.blend")
757 nodes))))] 661 nodes))))]
758 (fn [] 662 (fn []
759 (reduce concat (map relative-positions (list (first joints))))))) 663 (reduce concat (map relative-positions (list (first joints)))))))
760 664
761 665
666 (defn skel [node]
667 (doto
668 (.getSkeleton
669 (.getControl node SkeletonControl))
670 ;; this is necessary to force the skeleton to have accurate world
671 ;; transforms before it is rendered to the screen.
672 (.resetAndUpdate)))
673
674 (defn green-x-ray []
675 (doto (Material. (asset-manager)
676 "Common/MatDefs/Misc/Unshaded.j3md")
677 (.setColor "Color" ColorRGBA/Green)
678 (-> (.getAdditionalRenderState)
679 (.setDepthTest false))))
680
681 (defn test-worm []
682 (.start
683 (world
684 (doto (Node.)
685 ;;(.attachChild (point-worm))
686 (.attachChild (load-blender-model
687 "Models/anim2/joint-worm.blend"))
688
689 (.attachChild (box 10 1 10
690 :position (Vector3f. 0 -2 0) :mass 0
691 :color (ColorRGBA/Gray))))
692 {
693 "key-space" (fire-cannon-ball)
694 }
695 (fn [world]
696 (enable-debug world)
697 (light-up-everything world)
698 ;;(.setTimer world (NanoTimer.))
699 )
700 no-op)))
701
702
703
704 ;; defunct movement stuff
705 (defn torque-controls [control]
706 (let [torques
707 (concat
708 (map #(Vector3f. 0 (Math/sin %) (Math/cos %))
709 (range 0 (* Math/PI 2) (/ (* Math/PI 2) 20)))
710 [Vector3f/UNIT_X])]
711 (map (fn [torque-axis]
712 (fn [torque]
713 (.applyTorque
714 control
715 (.mult (.mult (.getPhysicsRotation control)
716 torque-axis)
717 (float
718 (* (.getMass control) torque))))))
719 torques)))
720
721 (defn motor-map
722 "Take a creature and generate a function that will enable fine
723 grained control over all the creature's limbs."
724 [#^Node creature]
725 (let [controls (keep #(.getControl % RigidBodyControl)
726 (node-seq creature))
727 limb-controls (reduce concat (map torque-controls controls))
728 body-control (partial map #(%1 %2) limb-controls)]
729 body-control))
730
731 (defn test-motor-map
732 "see how torque works."
733 []
734 (let [finger (box 3 0.5 0.5 :position (Vector3f. 0 2 0)
735 :mass 1 :color ColorRGBA/Green)
736 motor-map (motor-map finger)]
737 (world
738 (nodify [finger
739 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
740 :color ColorRGBA/Gray)])
741 standard-debug-controls
742 (fn [world]
743 (set-gravity world Vector3f/ZERO)
744 (light-up-everything world)
745 (.setTimer world (NanoTimer.)))
746 (fn [_ _]
747 (dorun (motor-map [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0]))))))
748
749
762 #+end_src 750 #+end_src
763 751
764 752
765 753
766 754