Mercurial > cortex

comparison org/body.org @ 135:421cc43441ae

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
cleaned up test, moved some functions
author Robert McIntyre <rlm@mit.edu>
date Wed, 01 Feb 2012 05:43:51 -0700
parents ac350a0ac6b0
children 47a4d74761f0
comparison
equal deleted inserted replaced
134:ac350a0ac6b0 135:421cc43441ae
13 (:import 13 (:import
14 com.jme3.math.Vector3f 14 com.jme3.math.Vector3f
15 com.jme3.math.Quaternion 15 com.jme3.math.Quaternion
16 com.jme3.math.Vector2f 16 com.jme3.math.Vector2f
17 com.jme3.math.Matrix3f 17 com.jme3.math.Matrix3f
18 com.jme3.bullet.control.RigidBodyControl)) 18 com.jme3.bullet.control.RigidBodyControl
19 com.jme3.collision.CollisionResults
20 com.jme3.bounding.BoundingBox))
19 21
20 (import com.jme3.scene.Node) 22 (import com.jme3.scene.Node)
21 23
24 (defn jme-to-blender
25 "Convert from JME coordinates to Blender coordinates"
26 [#^Vector3f in]
27 (Vector3f. (.getX in)
28 (- (.getZ in))
29 (.getY in)))
30
31 (defn joint-targets
32 "Return the two closest two objects to the joint object, ordered
33 from bottom to top according to the joint's rotation."
34 [#^Node parts #^Node joint]
35 (loop [radius (float 0.01)]
36 (let [results (CollisionResults.)]
37 (.collideWith
38 parts
39 (BoundingBox. (.getWorldTranslation joint)
40 radius radius radius)
41 results)
42 (let [targets
43 (distinct
44 (map #(.getGeometry %) results))]
45 (if (>= (count targets) 2)
46 (sort-by
47 #(let [v
48 (jme-to-blender
49 (.mult
50 (.inverse (.getWorldRotation joint))
51 (.subtract (.getWorldTranslation %)
52 (.getWorldTranslation joint))))]
53 (println-repl (.getName %) ":" v)
54 (.dot (Vector3f. 1 1 1)
55 v))
56 (take 2 targets))
57 (recur (float (* radius 2))))))))
58
59 (defn creature-joints
60 "Return the children of the creature's \"joints\" node."
61 [#^Node creature]
62 (if-let [joint-node (.getChild creature "joints")]
63 (seq (.getChildren joint-node))
64 (do (println-repl "could not find JOINTS node") [])))
65
22 (defn joint-proprioception [#^Node parts #^Node joint] 66 (defn joint-proprioception [#^Node parts #^Node joint]
23 (let [[obj-a obj-b] (cortex.silly/joint-targets parts joint) 67 (let [[obj-a obj-b] (joint-targets parts joint)
24 joint-rot (.getWorldRotation joint) 68 joint-rot (.getWorldRotation joint)
25 x (.mult joint-rot Vector3f/UNIT_X) 69 x (.mult joint-rot Vector3f/UNIT_X)
26 y (.mult joint-rot Vector3f/UNIT_Y) 70 y (.mult joint-rot Vector3f/UNIT_Y)
27 z (.mult joint-rot Vector3f/UNIT_Z)] 71 z (.mult joint-rot Vector3f/UNIT_Z)]
28 ;; this function will report proprioceptive information for the 72 ;; this function will report proprioceptive information for the
29 ;; joint 73 ;; joint.
30 (fn [] 74 (fn []
31 ;; x is the "twist" axis, y and z are the "bend" axes 75 ;; x is the "twist" axis, y and z are the "bend" axes
32 (let [rot-a (.getWorldRotation obj-a) 76 (let [rot-a (.getWorldRotation obj-a)
33 rot-b (.getWorldRotation obj-b) 77 rot-b (.getWorldRotation obj-b)
34 relative (.mult (.inverse rot-a) rot-b) 78 relative (.mult rot-b (.inverse rot-a))
35 basis (doto (Matrix3f.) 79 basis (doto (Matrix3f.)
36 (.setColumn 0 x) 80 (.setColumn 0 x)
37 (.setColumn 1 y) 81 (.setColumn 1 y)
38 (.setColumn 2 z)) 82 (.setColumn 2 z))
39 rotation-about-joint 83 rotation-about-joint
42 (.mult (.invert basis) 86 (.mult (.invert basis)
43 (.toRotationMatrix relative)))) 87 (.toRotationMatrix relative))))
44 [yaw roll pitch] 88 [yaw roll pitch]
45 (seq (.toAngles rotation-about-joint nil))] 89 (seq (.toAngles rotation-about-joint nil))]
46 ;;return euler angles of the quaternion around the new basis 90 ;;return euler angles of the quaternion around the new basis
47 ;;[yaw pitch roll]
48 [yaw roll pitch] 91 [yaw roll pitch]
49 )))) 92 ))))
50 93
51 94
52 (defn proprioception 95 (defn proprioception
53 "Create a function that provides proprioceptive information about an 96 "Create a function that provides proprioceptive information about an
54 entire body." 97 entire body."
55 [#^Node creature] 98 [#^Node creature]
56 ;; extract the body's joints 99 ;; extract the body's joints
57 (let [joints (cortex.silly/creature-joints creature) 100 (let [joints (creature-joints creature)
58 senses (map (partial joint-proprioception creature) joints)] 101 senses (map (partial joint-proprioception creature) joints)]
59 (fn [] 102 (fn []
60 (map #(%) senses)))) 103 (map #(%) senses))))
61 104
62 #+end_src 105 #+end_src
93 136
94 #+name: test-body 137 #+name: test-body
95 #+begin_src clojure 138 #+begin_src clojure
96 (ns cortex.test.body 139 (ns cortex.test.body
97 (:use (cortex world util body)) 140 (:use (cortex world util body))
141 (:require cortex.silly)
98 (:import 142 (:import
99 com.jme3.math.Vector3f 143 com.jme3.math.Vector3f
100 com.jme3.math.ColorRGBA 144 com.jme3.math.ColorRGBA
101 com.jme3.bullet.joints.Point2PointJoint 145 com.jme3.bullet.joints.Point2PointJoint
102 com.jme3.bullet.control.RigidBodyControl 146 com.jme3.bullet.control.RigidBodyControl
186 (Thread/sleep 20) 230 (Thread/sleep 20)
187 (dorun (worm-motor-map 231 (dorun (worm-motor-map
188 (worm-pattern @time))))))) 232 (worm-pattern @time)))))))
189 233
190 234
191 (require 'cortex.silly) 235
192 (defn join-at-point [obj-a obj-b world-pivot] 236 (defn join-at-point [obj-a obj-b world-pivot]
193 (cortex.silly/joint-dispatch 237 (cortex.silly/joint-dispatch
194 {:type :point} 238 {:type :point}
195 (.getControl obj-a RigidBodyControl) 239 (.getControl obj-a RigidBodyControl)
196 (.getControl obj-b RigidBodyControl) 240 (.getControl obj-b RigidBodyControl)
285 p y r))) 329 p y r)))
286 prop-data))) 330 prop-data)))
287 331
288 332
289 333
334 (defn tap [obj direction force]
335 (let [control (.getControl obj RigidBodyControl)]
336 (.applyTorque
337 control
338 (.mult (.getPhysicsRotation control)
339 (.mult (.normalize direction) (float force))))))
340
290 341
291 342
292 (defn test-proprioception 343 (defn test-proprioception
293 "Testing proprioception: 344 "Testing proprioception:
294 You should see two foating bars, and a printout of pitch, yaw, and 345 You should see two foating bars, and a printout of pitch, yaw, and
299 [] 350 []
300 (let [hand (box 1 0.2 0.2 :position (Vector3f. 0 2 0) 351 (let [hand (box 1 0.2 0.2 :position (Vector3f. 0 2 0)
301 :mass 0 :color ColorRGBA/Green :name "hand") 352 :mass 0 :color ColorRGBA/Green :name "hand")
302 finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0) 353 finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0)
303 :mass 1 :color ColorRGBA/Red :name "finger") 354 :mass 1 :color ColorRGBA/Red :name "finger")
304 floor (box 10 10 10 :position (Vector3f. 0 -15 0)
305 :mass 0 :color ColorRGBA/Gray)
306 joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow 355 joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow
307 :position (Vector3f. 1.2 2 0) 356 :position (Vector3f. 1.2 2 0)
308 :physical? false) 357 :physical? false)
358 joint (join-at-point hand finger (Vector3f. 1.2 2 0 ))
359 creature (nodify [hand finger joint-node])
360 ;; *******************************************
361 hand2 (box 1 0.2 0.2 :position (Vector3f. 0 1.5 -3)
362 :mass 0 :color ColorRGBA/Blue)
363 finger2 (box 1 0.2 0.2 :position (Vector3f. 2.4 1.5 -3)
364 :mass 1 :color ColorRGBA/Magenta)
365 joint-node2 (box 0.1 0.05 0.05 :color ColorRGBA/Gray
366 :position (Vector3f. 1.2 1.5 -3)
367 :physical? false)
368 joint2 (join-at-point hand2 finger2 (Vector3f. 1.2 1.5 -3))
369 creature2 (nodify [hand2 finger2 joint-node2])
370 ;; *******************************************
371
372 floor (box 10 10 10 :position (Vector3f. 0 -15 0)
373 :mass 0 :color ColorRGBA/Gray)
309 374
310 move-up? (atom false) 375 move-up? (atom false)
311 move-down? (atom false) 376 move-down? (atom false)
312 move-left? (atom false) 377 move-left? (atom false)
313 move-right? (atom false) 378 move-right? (atom false)
314 roll-left? (atom false) 379 roll-left? (atom false)
315 roll-right? (atom false) 380 roll-right? (atom false)
316 control (.getControl finger RigidBodyControl) 381
317 time (atom 0) 382
318 joint (join-at-point hand finger (Vector3f. 1.2 2 0 )) 383 root (nodify [creature creature2 floor])
319 creature (nodify [hand finger joint-node])
320 prop (joint-proprioception creature joint-node) 384 prop (joint-proprioception creature joint-node)
321 385 prop-view (proprioception-debug-window)]
322 prop-view (proprioception-debug-window)
323
324
325 ]
326 386
327 387
328 388
329 389 (comment
330 (.setCollisionGroup 390 (.setCollisionGroup
331 (.getControl hand RigidBodyControl) 391 (.getControl hand RigidBodyControl)
332 PhysicsCollisionObject/COLLISION_GROUP_NONE) 392 PhysicsCollisionObject/COLLISION_GROUP_NONE)
393 )
333 394
334 395
335 (world 396 (world
336 (nodify [hand finger floor joint-node]) 397 root
337 (merge standard-debug-controls 398 (merge standard-debug-controls
338 {"key-r" (fn [_ pressed?] (reset! move-up? pressed?)) 399 {"key-r" (fn [_ pressed?] (reset! move-up? pressed?))
339 "key-t" (fn [_ pressed?] (reset! move-down? pressed?)) 400 "key-t" (fn [_ pressed?] (reset! move-down? pressed?))
340 "key-f" (fn [_ pressed?] (reset! move-left? pressed?)) 401 "key-f" (fn [_ pressed?] (reset! move-left? pressed?))
341 "key-g" (fn [_ pressed?] (reset! move-right? pressed?)) 402 "key-g" (fn [_ pressed?] (reset! move-right? pressed?))
344 (fn [world] 405 (fn [world]
345 (.setTimer world (com.aurellem.capture.RatchetTimer. 60)) 406 (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
346 (set-gravity world (Vector3f. 0 0 0)) 407 (set-gravity world (Vector3f. 0 0 0))
347 (light-up-everything world)) 408 (light-up-everything world))
348 (fn [_ _] 409 (fn [_ _]
349 (if @move-up? 410 (let [force 10
350 (.applyTorque control 411 left (Vector3f. 0 1 0)
351 (.mult (.getPhysicsRotation control) 412 right (Vector3f. 0 -1 0)
352 (Vector3f. 0 0 10)))) 413 up (Vector3f. 0 0 1)
353 (if @move-down? 414 down (Vector3f. 0 0 -1)
354 (.applyTorque control 415 roll-left (Vector3f. -1 0 0)
355 (.mult (.getPhysicsRotation control) 416 roll-right (Vector3f. 1 0 0)]
356 (Vector3f. 0 0 -10)))) 417 (if @move-up? (tap finger up force))
357 (if @move-left? 418 (if @move-down? (tap finger down force))
358 (.applyTorque control 419 (if @move-left? (tap finger left force))
359 (.mult (.getPhysicsRotation control) 420 (if @move-right? (tap finger right force))
360 (Vector3f. 0 10 0)))) 421 (if @roll-left? (tap finger roll-left (/ force 10)))
361 (if @move-right? 422 (if @roll-right? (tap finger roll-right (/ force 10))))
362 (.applyTorque control 423 (prop-view (list (prop)))))))
363 (.mult (.getPhysicsRotation control)
364 (Vector3f. 0 -10 0))))
365 (if @roll-left?
366 (.applyTorque control
367 (.mult (.getPhysicsRotation control)
368 (Vector3f. -1 0 0))))
369 (if @roll-right?
370 (.applyTorque control
371 (.mult (.getPhysicsRotation control)
372 (Vector3f. 1 0 0))))
373
374 ;;(if (= 0 (rem (swap! time inc) 20))
375 (prop-view (list (prop)))))))
376 424
377 #+end_src 425 #+end_src
378 426
379 #+results: test-body 427 #+results: test-body
380 : #'cortex.test.body/test-proprioception 428 : #'cortex.test.body/test-proprioception