Mercurial > cortex

comparison org/body.org @ 141:22e193b5c60f

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
moved with-movement to cortex.body
author Robert McIntyre <rlm@mit.edu>
date Thu, 02 Feb 2012 01:35:21 -0700
parents 22444eb20ecc
children ccd057319c2a
comparison
equal deleted inserted replaced
140:22444eb20ecc 141:22e193b5c60f
104 ;; extract the body's joints 104 ;; extract the body's joints
105 (let [joints (creature-joints creature) 105 (let [joints (creature-joints creature)
106 senses (map (partial joint-proprioception creature) joints)] 106 senses (map (partial joint-proprioception creature) joints)]
107 (fn [] 107 (fn []
108 (map #(%) senses)))) 108 (map #(%) senses))))
109
110 #+end_src
111
112 #+results: proprioception
113 : #'cortex.body/proprioception
114
115 * Motor Control
116 #+name: motor-control
117 #+begin_src clojure
118 (in-ns 'cortex.body)
119
120 ;; surprisingly enough, terristerial creatures only move by using
121 ;; torque applied about their joints. There's not a single straight
122 ;; line of force in the human body at all! (A straight line of force
123 ;; would correspond to some sort of jet or rocket propulseion.)
124
125 (defn vector-motor-control
126 "Create a function that accepts a sequence of Vector3f objects that
127 describe the torque to be applied to each part of the body."
128 [body]
129 (let [nodes (node-seq body)
130 controls (keep #(.getControl % RigidBodyControl) nodes)]
131 (fn [torques]
132 (map #(.applyTorque %1 %2)
133 controls torques))))
134 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
135 #+end_src
136
137 ## note -- might want to add a lower dimensional, discrete version of
138 ## this if it proves useful from a x-modal clustering perspective.
139
140 * Examples
141
142 #+name: test-body
143 #+begin_src clojure
144 (ns cortex.test.body
145 (:use (cortex world util body))
146 (:require cortex.silly)
147 (:import
148 com.jme3.math.Vector3f
149 com.jme3.math.ColorRGBA
150 com.jme3.bullet.joints.Point2PointJoint
151 com.jme3.bullet.control.RigidBodyControl
152 com.jme3.system.NanoTimer))
153
154 (defn worm-segments
155 "Create multiple evenly spaced box segments. They're fabulous!"
156 [segment-length num-segments interstitial-space radius]
157 (letfn [(nth-segment
158 [n]
159 (box segment-length radius radius :mass 0.1
160 :position
161 (Vector3f.
162 (* 2 n (+ interstitial-space segment-length)) 0 0)
163 :name (str "worm-segment" n)
164 :color (ColorRGBA/randomColor)))]
165 (map nth-segment (range num-segments))))
166
167 (defn connect-at-midpoint
168 "Connect two physics objects with a Point2Point joint constraint at
169 the point equidistant from both objects' centers."
170 [segmentA segmentB]
171 (let [centerA (.getWorldTranslation segmentA)
172 centerB (.getWorldTranslation segmentB)
173 midpoint (.mult (.add centerA centerB) (float 0.5))
174 pivotA (.subtract midpoint centerA)
175 pivotB (.subtract midpoint centerB)
176
177 ;; A side-effect of creating a joint registers
178 ;; it with both physics objects which in turn
179 ;; will register the joint with the physics system
180 ;; when the simulation is started.
181 joint (Point2PointJoint.
182 (.getControl segmentA RigidBodyControl)
183 (.getControl segmentB RigidBodyControl)
184 pivotA
185 pivotB)]
186 segmentB))
187
188 (defn eve-worm
189 "Create a worm-like body bound by invisible joint constraints."
190 []
191 (let [segments (worm-segments 0.2 5 0.1 0.1)]
192 (dorun (map (partial apply connect-at-midpoint)
193 (partition 2 1 segments)))
194 (nodify "worm" segments)))
195
196 (defn worm-pattern
197 "This is a simple, mindless motor control pattern that drives the
198 second segment of the worm's body at an offset angle with
199 sinusoidally varying strength."
200 [time]
201 (let [angle (* Math/PI (/ 9 20))
202 direction (Vector3f. 0 (Math/sin angle) (Math/cos angle))]
203 [Vector3f/ZERO
204 (.mult
205 direction
206 (float (* 2 (Math/sin (* Math/PI 2 (/ (rem time 300 ) 300))))))
207 Vector3f/ZERO
208 Vector3f/ZERO
209 Vector3f/ZERO]))
210
211 (defn test-motor-control
212 "Testing motor-control:
213 You should see a multi-segmented worm-like object fall onto the
214 table and begin writhing and moving."
215 []
216 (let [worm (eve-worm)
217 time (atom 0)
218 worm-motor-map (vector-motor-control worm)]
219 (world
220 (nodify [worm
221 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
222 :color ColorRGBA/Gray)])
223 standard-debug-controls
224 (fn [world]
225 (enable-debug world)
226 (light-up-everything world)
227 (comment
228 (com.aurellem.capture.Capture/captureVideo
229 world
230 (file-str "/home/r/proj/cortex/tmp/moving-worm")))
231 )
232
233 (fn [_ _]
234 (swap! time inc)
235 (Thread/sleep 20)
236 (dorun (worm-motor-map
237 (worm-pattern @time)))))))
238
239
240
241 (defn join-at-point [obj-a obj-b world-pivot]
242 (cortex.silly/joint-dispatch
243 {:type :point}
244 (.getControl obj-a RigidBodyControl)
245 (.getControl obj-b RigidBodyControl)
246 (cortex.silly/world-to-local obj-a world-pivot)
247 (cortex.silly/world-to-local obj-b world-pivot)
248 nil
249 ))
250
251 (import com.jme3.bullet.collision.PhysicsCollisionObject)
252
253 (defn blab-* []
254 (let [hand (box 0.5 0.2 0.2 :position (Vector3f. 0 0 0)
255 :mass 0 :color ColorRGBA/Green)
256 finger (box 0.5 0.2 0.2 :position (Vector3f. 2.4 0 0)
257 :mass 1 :color ColorRGBA/Red)
258 connection-point (Vector3f. 1.2 0 0)
259 root (nodify [hand finger])]
260
261 (join-at-point hand finger (Vector3f. 1.2 0 0))
262
263 (.setCollisionGroup
264 (.getControl hand RigidBodyControl)
265 PhysicsCollisionObject/COLLISION_GROUP_NONE)
266 (world
267 root
268 standard-debug-controls
269 (fn [world]
270 (enable-debug world)
271 (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
272 (set-gravity world Vector3f/ZERO)
273 )
274 no-op)))
275 (import java.awt.image.BufferedImage)
276
277 (defn draw-sprite [image sprite x y color ]
278 (dorun
279 (for [[u v] sprite]
280 (.setRGB image (+ u x) (+ v y) color))))
281
282 (defn view-angle
283 "create a debug view of an angle"
284 [color]
285 (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
286 previous (atom [25 25])
287 sprite [[0 0] [0 1]
288 [0 -1] [-1 0] [1 0]]]
289 (fn [angle]
290 (let [angle (float angle)]
291 (let [position
292 [(+ 25 (int (* 20 (Math/cos angle))))
293 (+ 25 (int (* 20(Math/sin angle))))]]
294 (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)
295 (draw-sprite image sprite (position 0) (position 1) color)
296 (reset! previous position))
297 image))))
298
299 (defn proprioception-debug-window
300 []
301 (let [yaw (view-angle 0xFF0000)
302 roll (view-angle 0x00FF00)
303 pitch (view-angle 0xFFFFFF)
304 v-yaw (view-image)
305 v-roll (view-image)
306 v-pitch (view-image)
307 ]
308 (fn [prop-data]
309 (dorun
310 (map
311 (fn [[y r p]]
312 (v-yaw (yaw y))
313 (v-roll (roll r))
314 (v-pitch (pitch p)))
315 prop-data)))))
316 (comment
317
318 (defn proprioception-debug-window
319 []
320 (let [time (atom 0)]
321 (fn [prop-data]
322 (if (= 0 (rem (swap! time inc) 40))
323 (println-repl prop-data)))))
324 )
325
326 (comment
327 (dorun
328 (map
329 (comp
330 println-repl
331 (fn [[p y r]]
332 (format
333 "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n"
334 p y r)))
335 prop-data)))
336
337
338 109
339 (defn tap [obj direction force] 110 (defn tap [obj direction force]
340 (let [control (.getControl obj RigidBodyControl)] 111 (let [control (.getControl obj RigidBodyControl)]
341 (.applyTorque 112 (.applyTorque
342 control 113 control
343 (.mult (.getPhysicsRotation control) 114 (.mult (.getPhysicsRotation control)
344 (.mult (.normalize direction) (float force)))))) 115 (.mult (.normalize direction) (float force))))))
345
346 116
347 117
348 (defn with-movement 118 (defn with-movement
349 [object 119 [object
350 [up down left right roll-up roll-down :as keyboard] 120 [up down left right roll-up roll-down :as keyboard]
391 161
392 [root-node 162 [root-node
393 keymap* 163 keymap*
394 intilization 164 intilization
395 world-loop*])) 165 world-loop*]))
166
167
168 #+end_src
169
170 #+results: proprioception
171 : #'cortex.body/proprioception
172
173 * Motor Control
174 #+name: motor-control
175 #+begin_src clojure
176 (in-ns 'cortex.body)
177
178 ;; surprisingly enough, terristerial creatures only move by using
179 ;; torque applied about their joints. There's not a single straight
180 ;; line of force in the human body at all! (A straight line of force
181 ;; would correspond to some sort of jet or rocket propulseion.)
182
183 (defn vector-motor-control
184 "Create a function that accepts a sequence of Vector3f objects that
185 describe the torque to be applied to each part of the body."
186 [body]
187 (let [nodes (node-seq body)
188 controls (keep #(.getControl % RigidBodyControl) nodes)]
189 (fn [torques]
190 (map #(.applyTorque %1 %2)
191 controls torques))))
192 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
193 #+end_src
194
195 ## note -- might want to add a lower dimensional, discrete version of
196 ## this if it proves useful from a x-modal clustering perspective.
197
198 * Examples
199
200 #+name: test-body
201 #+begin_src clojure
202 (ns cortex.test.body
203 (:use (cortex world util body))
204 (:require cortex.silly)
205 (:import
206 com.jme3.math.Vector3f
207 com.jme3.math.ColorRGBA
208 com.jme3.bullet.joints.Point2PointJoint
209 com.jme3.bullet.control.RigidBodyControl
210 com.jme3.system.NanoTimer))
211
212 (defn worm-segments
213 "Create multiple evenly spaced box segments. They're fabulous!"
214 [segment-length num-segments interstitial-space radius]
215 (letfn [(nth-segment
216 [n]
217 (box segment-length radius radius :mass 0.1
218 :position
219 (Vector3f.
220 (* 2 n (+ interstitial-space segment-length)) 0 0)
221 :name (str "worm-segment" n)
222 :color (ColorRGBA/randomColor)))]
223 (map nth-segment (range num-segments))))
224
225 (defn connect-at-midpoint
226 "Connect two physics objects with a Point2Point joint constraint at
227 the point equidistant from both objects' centers."
228 [segmentA segmentB]
229 (let [centerA (.getWorldTranslation segmentA)
230 centerB (.getWorldTranslation segmentB)
231 midpoint (.mult (.add centerA centerB) (float 0.5))
232 pivotA (.subtract midpoint centerA)
233 pivotB (.subtract midpoint centerB)
234
235 ;; A side-effect of creating a joint registers
236 ;; it with both physics objects which in turn
237 ;; will register the joint with the physics system
238 ;; when the simulation is started.
239 joint (Point2PointJoint.
240 (.getControl segmentA RigidBodyControl)
241 (.getControl segmentB RigidBodyControl)
242 pivotA
243 pivotB)]
244 segmentB))
245
246 (defn eve-worm
247 "Create a worm-like body bound by invisible joint constraints."
248 []
249 (let [segments (worm-segments 0.2 5 0.1 0.1)]
250 (dorun (map (partial apply connect-at-midpoint)
251 (partition 2 1 segments)))
252 (nodify "worm" segments)))
253
254 (defn worm-pattern
255 "This is a simple, mindless motor control pattern that drives the
256 second segment of the worm's body at an offset angle with
257 sinusoidally varying strength."
258 [time]
259 (let [angle (* Math/PI (/ 9 20))
260 direction (Vector3f. 0 (Math/sin angle) (Math/cos angle))]
261 [Vector3f/ZERO
262 (.mult
263 direction
264 (float (* 2 (Math/sin (* Math/PI 2 (/ (rem time 300 ) 300))))))
265 Vector3f/ZERO
266 Vector3f/ZERO
267 Vector3f/ZERO]))
268
269 (defn test-motor-control
270 "Testing motor-control:
271 You should see a multi-segmented worm-like object fall onto the
272 table and begin writhing and moving."
273 []
274 (let [worm (eve-worm)
275 time (atom 0)
276 worm-motor-map (vector-motor-control worm)]
277 (world
278 (nodify [worm
279 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
280 :color ColorRGBA/Gray)])
281 standard-debug-controls
282 (fn [world]
283 (enable-debug world)
284 (light-up-everything world)
285 (comment
286 (com.aurellem.capture.Capture/captureVideo
287 world
288 (file-str "/home/r/proj/cortex/tmp/moving-worm")))
289 )
290
291 (fn [_ _]
292 (swap! time inc)
293 (Thread/sleep 20)
294 (dorun (worm-motor-map
295 (worm-pattern @time)))))))
296
297
298
299 (defn join-at-point [obj-a obj-b world-pivot]
300 (cortex.silly/joint-dispatch
301 {:type :point}
302 (.getControl obj-a RigidBodyControl)
303 (.getControl obj-b RigidBodyControl)
304 (cortex.silly/world-to-local obj-a world-pivot)
305 (cortex.silly/world-to-local obj-b world-pivot)
306 nil
307 ))
308
309 (import com.jme3.bullet.collision.PhysicsCollisionObject)
310
311 (defn blab-* []
312 (let [hand (box 0.5 0.2 0.2 :position (Vector3f. 0 0 0)
313 :mass 0 :color ColorRGBA/Green)
314 finger (box 0.5 0.2 0.2 :position (Vector3f. 2.4 0 0)
315 :mass 1 :color ColorRGBA/Red)
316 connection-point (Vector3f. 1.2 0 0)
317 root (nodify [hand finger])]
318
319 (join-at-point hand finger (Vector3f. 1.2 0 0))
320
321 (.setCollisionGroup
322 (.getControl hand RigidBodyControl)
323 PhysicsCollisionObject/COLLISION_GROUP_NONE)
324 (world
325 root
326 standard-debug-controls
327 (fn [world]
328 (enable-debug world)
329 (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
330 (set-gravity world Vector3f/ZERO)
331 )
332 no-op)))
333 (import java.awt.image.BufferedImage)
334
335 (defn draw-sprite [image sprite x y color ]
336 (dorun
337 (for [[u v] sprite]
338 (.setRGB image (+ u x) (+ v y) color))))
339
340 (defn view-angle
341 "create a debug view of an angle"
342 [color]
343 (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
344 previous (atom [25 25])
345 sprite [[0 0] [0 1]
346 [0 -1] [-1 0] [1 0]]]
347 (fn [angle]
348 (let [angle (float angle)]
349 (let [position
350 [(+ 25 (int (* 20 (Math/cos angle))))
351 (+ 25 (int (* 20(Math/sin angle))))]]
352 (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)
353 (draw-sprite image sprite (position 0) (position 1) color)
354 (reset! previous position))
355 image))))
356
357 (defn proprioception-debug-window
358 []
359 (let [yaw (view-angle 0xFF0000)
360 roll (view-angle 0x00FF00)
361 pitch (view-angle 0xFFFFFF)
362 v-yaw (view-image)
363 v-roll (view-image)
364 v-pitch (view-image)
365 ]
366 (fn [prop-data]
367 (dorun
368 (map
369 (fn [[y r p]]
370 (v-yaw (yaw y))
371 (v-roll (roll r))
372 (v-pitch (pitch p)))
373 prop-data)))))
374 (comment
375
376 (defn proprioception-debug-window
377 []
378 (let [time (atom 0)]
379 (fn [prop-data]
380 (if (= 0 (rem (swap! time inc) 40))
381 (println-repl prop-data)))))
382 )
383
384 (comment
385 (dorun
386 (map
387 (comp
388 println-repl
389 (fn [[p y r]]
390 (format
391 "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n"
392 p y r)))
393 prop-data)))
394
395
396 396
397 397
398 (defn test-proprioception 398 (defn test-proprioception
399 "Testing proprioception: 399 "Testing proprioception:
400 You should see two foating bars, and a printout of pitch, yaw, and 400 You should see two foating bars, and a printout of pitch, yaw, and