Mercurial > cortex

comparison org/body.org @ 64:ab1fee4280c3

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
divided body into test suite and core functions
author Robert McIntyre <rlm@mit.edu>
date Wed, 30 Nov 2011 20:42:07 -0700
parents 7f2653ad3199
children 4b5f00110d8c
comparison
equal deleted inserted replaced
63:7f2653ad3199 64:ab1fee4280c3
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 * COMMENT Body 8
9 9
10 #+srcname: body-main 10 * Proprioception
11 #+srcname: proprioception
11 #+begin_src clojure 12 #+begin_src clojure
12 (ns cortex.body 13 (ns cortex.body
13 (use (cortex world util import))) 14 (:use (cortex world util))
14 15 (:import
15 (use 'clojure.contrib.def) 16 com.jme3.math.Vector3f
16 (cortex.import/mega-import-jme3) 17 com.jme3.math.Quaternion
17 (rlm.rlm-commands/help) 18 com.jme3.math.Vector2f
18 19 com.jme3.math.Matrix3f
19 (defn load-blender-model 20 com.jme3.bullet.control.RigidBodyControl))
20 "Load a .blend file using an asset folder relative path." 21
21 [^String model] 22 (defn any-orthogonal
22 (.loadModel
23 (doto (asset-manager)
24 (.registerLoader BlenderModelLoader (into-array String ["blend"])))
25 model))
26
27 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
28
29 ;;;;;;;;;;;; eve-style bodies ;;;;;;;;
30
31 (defn worm-segments
32 "Create multiple evenly spaced box segments. They're fabulous!"
33 [segment-length num-segments interstitial-space radius]
34 (letfn [(nth-segment
35 [n]
36 (box segment-length radius radius :mass 0.1
37 :position
38 (Vector3f.
39 (* 2 n (+ interstitial-space segment-length)) 0 0)
40 :name (str "worm-segment" n)
41 :color (ColorRGBA/randomColor)))]
42 (map nth-segment (range num-segments))))
43
44 (defn connect-at-midpoint
45 "Connect two physics objects with a Point2Point joint constraint at
46 the point equidistant from both objects' centers."
47 [segmentA segmentB]
48 (let [centerA (.getWorldTranslation segmentA)
49 centerB (.getWorldTranslation segmentB)
50 midpoint (.mult (.add centerA centerB) (float 0.5))
51 pivotA (.subtract midpoint centerA)
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.
58 joint (Point2PointJoint.
59 (.getControl segmentA RigidBodyControl)
60 (.getControl segmentB RigidBodyControl)
61 pivotA
62 pivotB)]
63 segmentB))
64
65 (defn point-worm []
66 (let [segments (worm-segments 0.2 5 0.1 0.1)]
67 (dorun (map (partial apply connect-at-midpoint)
68 (partition 2 1 segments)))
69 (nodify "worm" segments)))
70
71 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
72 ;;;;;;;;; Proprioception ;;;;;;;;;;;;;
73 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
74
75 (declare
76 ;; generate an arbitray (but stable) orthogonal vector
77 ;; to a given vector.
78 some-orthogonal
79
80 ;; determine the amount of rotation a quaternion will
81 ;; cause about a given axis
82 project-quaternion
83
84 ;; proprioception for a single joint
85 joint-proprioception
86
87 ;; create a function that provides proprioceptive information
88 ;; about an entire body.
89 proprioception)
90
91 (defn some-orthogonal
92 "Generate an arbitray (but stable) orthogonal vector to a given 23 "Generate an arbitray (but stable) orthogonal vector to a given
93 vector." 24 vector."
94 [vector] 25 [vector]
95 (let [x (.getX vector) 26 (let [x (.getX vector)
96 y (.getY vector) 27 y (.getY vector)
106 component-of-a-quaternion-rotation-around-an-axis. 37 component-of-a-quaternion-rotation-around-an-axis.
107 38
108 Determine the amount of rotation a quaternion will 39 Determine the amount of rotation a quaternion will
109 cause about a given axis." 40 cause about a given axis."
110 [#^Quaternion q #^Vector3f axis] 41 [#^Quaternion q #^Vector3f axis]
111 (let [basis-1 (orthogonal-vect axis) 42 (let [basis-1 (any-orthogonal axis)
112 basis-2 (.cross axis basis-1) 43 basis-2 (.cross axis basis-1)
113 rotated (.mult q basis-1) 44 rotated (.mult q basis-1)
114 alpha (.dot basis-1 (.project rotated basis-1)) 45 alpha (.dot basis-1 (.project rotated basis-1))
115 beta (.dot basis-2 (.project rotated basis-2))] 46 beta (.dot basis-2 (.project rotated basis-2))]
116 (Math/atan2 beta alpha))) 47 (Math/atan2 beta alpha)))
145 (.angleBetween 76 (.angleBetween
146 (.normalize (Vector2f. (.getX arm-b) (.getZ arm-b))) 77 (.normalize (Vector2f. (.getX arm-b) (.getZ arm-b)))
147 (Vector2f. 1 0)) 78 (Vector2f. 1 0))
148 79
149 roll 80 roll
150 (rot-about-axis 81 (project-quaternion
151 (.mult 82 (.mult
152 (.getLocalRotation object-b) 83 (.getLocalRotation object-b)
153 (doto (Quaternion.) 84 (doto (Quaternion.)
154 (.fromRotationMatrix rotate-a))) 85 (.fromRotationMatrix rotate-a)))
155 arm-b)] 86 arm-b)]
169 #(.getControl % RigidBodyControl) 100 #(.getControl % RigidBodyControl)
170 (node-seq body)))))] 101 (node-seq body)))))]
171 (fn [] 102 (fn []
172 (map joint-proprioception joints)))) 103 (map joint-proprioception joints))))
173 104
174 105 #+end_src
175 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 106
176 ;;;;;;;;; Mortor Control ;;;;;;;;;;;;; 107 #+srcname: motor-control
177 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 108 #+begin_src clojure
178 109 (in-ns 'cortex.body)
179 110
180 ;; surprisingly enough, terristerial creatures only move by using 111 ;; surprisingly enough, terristerial creatures only move by using
181 ;; torque applied about their joints. There's not a single straight 112 ;; 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 113 ;; 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.) 114 ;; would correspond to some sort of jet or rocket propulseion.)
189 (let [nodes (node-seq body) 120 (let [nodes (node-seq body)
190 controls (keep #(.getControl % RigidBodyControl) nodes)] 121 controls (keep #(.getControl % RigidBodyControl) nodes)]
191 (fn [torques] 122 (fn [torques]
192 (map #(.applyTorque %1 %2) 123 (map #(.applyTorque %1 %2)
193 controls torques)))) 124 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 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 125 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
199 126 #+end_src
200 127
201 128 ## note -- might want to add a lower dimensional, discrete version of
202 (defn worm-pattern [time] 129 ## this if it proves useful from a x-modal clustering perspective.
203 [ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 130
204 131 * Examples
205 0 0 0 0 0 0 0 0 0 0 0 132
206 133 #+srcname: test-body
207 (* 20 (Math/sin (* Math/PI 2 (/ (rem time 300 ) 300)))) 134 #+begin_src clojure
208 135 (ns test.body
209 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 136 (:use (cortex world util body))
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 137 (:import
211 0 0 0 0 0 0 0 0 0 0 0 0 0 0 138 com.jme3.math.Vector3f
212 139 com.jme3.math.ColorRGBA
213 ]) 140 com.jme3.bullet.joints.Point2PointJoint
214 141 com.jme3.bullet.control.RigidBodyControl
215 (defn worm-pattern [time] 142 com.jme3.system.NanoTimer))
216 (let [angle (* Math/PI (/ 4 20)) 143
144 (defn worm-segments
145 "Create multiple evenly spaced box segments. They're fabulous!"
146 [segment-length num-segments interstitial-space radius]
147 (letfn [(nth-segment
148 [n]
149 (box segment-length radius radius :mass 0.1
150 :position
151 (Vector3f.
152 (* 2 n (+ interstitial-space segment-length)) 0 0)
153 :name (str "worm-segment" n)
154 :color (ColorRGBA/randomColor)))]
155 (map nth-segment (range num-segments))))
156
157 (defn connect-at-midpoint
158 "Connect two physics objects with a Point2Point joint constraint at
159 the point equidistant from both objects' centers."
160 [segmentA segmentB]
161 (let [centerA (.getWorldTranslation segmentA)
162 centerB (.getWorldTranslation segmentB)
163 midpoint (.mult (.add centerA centerB) (float 0.5))
164 pivotA (.subtract midpoint centerA)
165 pivotB (.subtract midpoint centerB)
166
167 ;; A side-effect of creating a joint registers
168 ;; it with both physics objects which in turn
169 ;; will register the joint with the physics system
170 ;; when the simulation is started.
171 joint (Point2PointJoint.
172 (.getControl segmentA RigidBodyControl)
173 (.getControl segmentB RigidBodyControl)
174 pivotA
175 pivotB)]
176 segmentB))
177
178 (defn eve-worm
179 "Create a worm body bound by invisible joint constraints."
180 []
181 (let [segments (worm-segments 0.2 5 0.1 0.1)]
182 (dorun (map (partial apply connect-at-midpoint)
183 (partition 2 1 segments)))
184 (nodify "worm" segments)))
185
186 (defn worm-pattern
187 "This is a simple, mindless motor control pattern that drives the
188 second segment of the worm's body at an offset angle with
189 sinusoidally varying strength."
190 [time]
191 (let [angle (* Math/PI (/ 9 20))
217 direction (Vector3f. 0 (Math/sin angle) (Math/cos angle))] 192 direction (Vector3f. 0 (Math/sin angle) (Math/cos angle))]
218 [Vector3f/ZERO 193 [Vector3f/ZERO
219 (.mult 194 (.mult
220 direction 195 direction
221 (float (* 2 (Math/sin (* Math/PI 2 (/ (rem time 300 ) 300)))))) 196 (float (* 2 (Math/sin (* Math/PI 2 (/ (rem time 300 ) 300))))))
222 Vector3f/ZERO 197 Vector3f/ZERO
223 Vector3f/ZERO 198 Vector3f/ZERO
224 Vector3f/ZERO])) 199 Vector3f/ZERO]))
225 200
226 (defn test-worm-control 201 (defn test-motor-control
227 [] 202 "You should see a multi-segmented worm-like object fall onto the
228 (let [worm (point-worm) 203 table and begin writhing and moving."
204 []
205 (let [worm (eve-worm)
229 time (atom 0) 206 time (atom 0)
230 worm-motor-map (vector-motor-control worm)] 207 worm-motor-map (vector-motor-control worm)]
231 (world 208 (world
232 (nodify [worm 209 (nodify [worm
233 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0 210 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
242 (file-str "/home/r/proj/cortex/tmp/moving-worm"))) 219 (file-str "/home/r/proj/cortex/tmp/moving-worm")))
243 ) 220 )
244 221
245 (fn [_ _] 222 (fn [_ _]
246 (swap! time inc) 223 (swap! time inc)
247 ;;(Thread/sleep 200) 224 (Thread/sleep 20)
248 (dorun (worm-motor-map 225 (dorun (worm-motor-map
249 (worm-pattern @time))))))) 226 (worm-pattern @time)))))))
250 227
251 228 (defn test-proprioception
252 229 "You should see two foating bars, and a printout of pitch, yaw, and
253 230 roll. Pressing key-r/key-t should move the blue bar up and down and
254 (defn test-prop 231 change only the value of pitch. key-f/key-g moves it side to side
255 "see how torque works." 232 and changes yaw. key-v/key-b will spin the blue segment clockwise
233 and counterclockwise, and only affect roll."
256 [] 234 []
257 (let [hand (box 1 0.2 0.2 :position (Vector3f. 0 2 0) 235 (let [hand (box 1 0.2 0.2 :position (Vector3f. 0 2 0)
258 :mass 0 :color ColorRGBA/Green) 236 :mass 0 :color ColorRGBA/Green)
259 finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0) 237 finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0)
260 :mass 1 :color (ColorRGBA. 0.20 0.40 0.99 1.0)) 238 :mass 1 :color (ColorRGBA. 0.20 0.40 0.99 1.0))
274 (.getControl hand RigidBodyControl) 252 (.getControl hand RigidBodyControl)
275 control 253 control
276 (Vector3f. 1.2 0 0) 254 (Vector3f. 1.2 0 0)
277 (Vector3f. -1.2 0 0 )) 255 (Vector3f. -1.2 0 0 ))
278 (.setCollisionBetweenLinkedBodys false)) 256 (.setCollisionBetweenLinkedBodys false))
279 time (atom 0) 257 time (atom 0)]
280 ]
281 (world 258 (world
282 (nodify [hand finger floor]) 259 (nodify [hand finger floor])
283 (merge standard-debug-controls 260 (merge standard-debug-controls
284 {"key-r" (fn [_ pressed?] (reset! move-up? pressed?)) 261 {"key-r" (fn [_ pressed?] (reset! move-up? pressed?))
285 "key-t" (fn [_ pressed?] (reset! move-down? pressed?)) 262 "key-t" (fn [_ pressed?] (reset! move-down? pressed?))
325 302
326 (apply 303 (apply
327 (comp 304 (comp
328 println-repl 305 println-repl
329 #(format "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n" %1 %2 %3)) 306 #(format "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n" %1 %2 %3))
330 (relative-positions joint)))))))) 307 (joint-proprioception joint))))))))
331
332 #+end_src 308 #+end_src
309
310 #+results: test-body
311 : #'test.body/test-proprioception
312
333 313
334 314
335 * COMMENT code-limbo 315 * COMMENT code-limbo
336 #+begin_src clojure 316 #+begin_src clojure
337 ;;(.loadModel 317 ;;(.loadModel
338 ;; (doto (asset-manager) 318 ;; (doto (asset-manager)
339 ;; (.registerLoader BlenderModelLoader (into-array String ["blend"]))) 319 ;; (.registerLoader BlenderModelLoader (into-array String ["blend"])))
340 ;; "Models/person/person.blend") 320 ;; "Models/person/person.blend")
321
322
323 (defn load-blender-model
324 "Load a .blend file using an asset folder relative path."
325 [^String model]
326 (.loadModel
327 (doto (asset-manager)
328 (.registerLoader BlenderModelLoader (into-array String ["blend"])))
329 model))
330
341 331
342 (defn view-model [^String model] 332 (defn view-model [^String model]
343 (view 333 (view
344 (.loadModel 334 (.loadModel
345 (doto (asset-manager) 335 (doto (asset-manager)
755 745
756 746
757 747
758 * COMMENT generate Source. 748 * COMMENT generate Source.
759 #+begin_src clojure :tangle ../src/cortex/body.clj 749 #+begin_src clojure :tangle ../src/cortex/body.clj
760 <<body-main>> 750 <<proprioception>>
751 <<motor-control>>
761 #+end_src 752 #+end_src
753
754 #+begin_src clojure :tangle ../src/test/body.clj
755 <<test-body>>
756 #+end_src
757
758
762 759
763