Mercurial > cortex

annotate org/body.org @ 134:ac350a0ac6b0

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
proprioception refrence frame is wrong, trying to fix...
author Robert McIntyre <rlm@mit.edu>
date Wed, 01 Feb 2012 02:44:07 -0700
parents 2ed7e60d3821
children 421cc43441ae
rev   line source
rlm@0 1 #+title: The BODY!!!
rlm@0 2 #+author: Robert McIntyre
rlm@0 3 #+email: rlm@mit.edu
rlm@4 4 #+description: Simulating a body (movement, touch, propioception) in jMonkeyEngine3.
rlm@4 5 #+SETUPFILE: ../../aurellem/org/setup.org
rlm@4 6 #+INCLUDE: ../../aurellem/org/level-0.org
rlm@4 7
rlm@64 8 * Proprioception
rlm@66 9 #+name: proprioception
rlm@0 10 #+begin_src clojure
rlm@44 11 (ns cortex.body
rlm@64 12 (:use (cortex world util))
rlm@64 13 (:import
rlm@64 14 com.jme3.math.Vector3f
rlm@64 15 com.jme3.math.Quaternion
rlm@64 16 com.jme3.math.Vector2f
rlm@64 17 com.jme3.math.Matrix3f
rlm@64 18 com.jme3.bullet.control.RigidBodyControl))
rlm@44 19
rlm@133 20 (import com.jme3.scene.Node)
rlm@133 21
rlm@133 22 (defn joint-proprioception [#^Node parts #^Node joint]
rlm@133 23 (let [[obj-a obj-b] (cortex.silly/joint-targets parts joint)
rlm@133 24 joint-rot (.getWorldRotation joint)
rlm@133 25 x (.mult joint-rot Vector3f/UNIT_X)
rlm@133 26 y (.mult joint-rot Vector3f/UNIT_Y)
rlm@133 27 z (.mult joint-rot Vector3f/UNIT_Z)]
rlm@133 28 ;; this function will report proprioceptive information for the
rlm@133 29 ;; joint
rlm@133 30 (fn []
rlm@133 31 ;; x is the "twist" axis, y and z are the "bend" axes
rlm@133 32 (let [rot-a (.getWorldRotation obj-a)
rlm@133 33 rot-b (.getWorldRotation obj-b)
rlm@133 34 relative (.mult (.inverse rot-a) rot-b)
rlm@133 35 basis (doto (Matrix3f.)
rlm@133 36 (.setColumn 0 x)
rlm@133 37 (.setColumn 1 y)
rlm@133 38 (.setColumn 2 z))
rlm@133 39 rotation-about-joint
rlm@133 40 (doto (Quaternion.)
rlm@133 41 (.fromRotationMatrix
rlm@133 42 (.mult (.invert basis)
rlm@133 43 (.toRotationMatrix relative))))
rlm@133 44 [yaw roll pitch]
rlm@133 45 (seq (.toAngles rotation-about-joint nil))]
rlm@133 46 ;;return euler angles of the quaternion around the new basis
rlm@133 47 ;;[yaw pitch roll]
rlm@133 48 [yaw roll pitch]
rlm@133 49 ))))
rlm@133 50
rlm@133 51
rlm@63 52 (defn proprioception
rlm@63 53 "Create a function that provides proprioceptive information about an
rlm@63 54 entire body."
rlm@134 55 [#^Node creature]
rlm@63 56 ;; extract the body's joints
rlm@134 57 (let [joints (cortex.silly/creature-joints creature)
rlm@134 58 senses (map (partial joint-proprioception creature) joints)]
rlm@63 59 (fn []
rlm@134 60 (map #(%) senses))))
rlm@60 61
rlm@64 62 #+end_src
rlm@63 63
rlm@133 64 #+results: proprioception
rlm@133 65 : #'cortex.body/proprioception
rlm@133 66
rlm@65 67 * Motor Control
rlm@66 68 #+name: motor-control
rlm@64 69 #+begin_src clojure
rlm@64 70 (in-ns 'cortex.body)
rlm@63 71
rlm@63 72 ;; surprisingly enough, terristerial creatures only move by using
rlm@63 73 ;; torque applied about their joints. There's not a single straight
rlm@63 74 ;; line of force in the human body at all! (A straight line of force
rlm@63 75 ;; would correspond to some sort of jet or rocket propulseion.)
rlm@63 76
rlm@63 77 (defn vector-motor-control
rlm@63 78 "Create a function that accepts a sequence of Vector3f objects that
rlm@63 79 describe the torque to be applied to each part of the body."
rlm@63 80 [body]
rlm@63 81 (let [nodes (node-seq body)
rlm@63 82 controls (keep #(.getControl % RigidBodyControl) nodes)]
rlm@63 83 (fn [torques]
rlm@63 84 (map #(.applyTorque %1 %2)
rlm@63 85 controls torques))))
rlm@64 86 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@64 87 #+end_src
rlm@64 88
rlm@64 89 ## note -- might want to add a lower dimensional, discrete version of
rlm@64 90 ## this if it proves useful from a x-modal clustering perspective.
rlm@63 91
rlm@64 92 * Examples
rlm@63 93
rlm@69 94 #+name: test-body
rlm@64 95 #+begin_src clojure
rlm@69 96 (ns cortex.test.body
rlm@64 97 (:use (cortex world util body))
rlm@64 98 (:import
rlm@64 99 com.jme3.math.Vector3f
rlm@64 100 com.jme3.math.ColorRGBA
rlm@64 101 com.jme3.bullet.joints.Point2PointJoint
rlm@64 102 com.jme3.bullet.control.RigidBodyControl
rlm@64 103 com.jme3.system.NanoTimer))
rlm@63 104
rlm@64 105 (defn worm-segments
rlm@64 106 "Create multiple evenly spaced box segments. They're fabulous!"
rlm@64 107 [segment-length num-segments interstitial-space radius]
rlm@64 108 (letfn [(nth-segment
rlm@64 109 [n]
rlm@64 110 (box segment-length radius radius :mass 0.1
rlm@64 111 :position
rlm@64 112 (Vector3f.
rlm@64 113 (* 2 n (+ interstitial-space segment-length)) 0 0)
rlm@64 114 :name (str "worm-segment" n)
rlm@64 115 :color (ColorRGBA/randomColor)))]
rlm@64 116 (map nth-segment (range num-segments))))
rlm@63 117
rlm@64 118 (defn connect-at-midpoint
rlm@64 119 "Connect two physics objects with a Point2Point joint constraint at
rlm@64 120 the point equidistant from both objects' centers."
rlm@64 121 [segmentA segmentB]
rlm@64 122 (let [centerA (.getWorldTranslation segmentA)
rlm@64 123 centerB (.getWorldTranslation segmentB)
rlm@64 124 midpoint (.mult (.add centerA centerB) (float 0.5))
rlm@64 125 pivotA (.subtract midpoint centerA)
rlm@64 126 pivotB (.subtract midpoint centerB)
rlm@64 127
rlm@64 128 ;; A side-effect of creating a joint registers
rlm@64 129 ;; it with both physics objects which in turn
rlm@64 130 ;; will register the joint with the physics system
rlm@64 131 ;; when the simulation is started.
rlm@64 132 joint (Point2PointJoint.
rlm@64 133 (.getControl segmentA RigidBodyControl)
rlm@64 134 (.getControl segmentB RigidBodyControl)
rlm@64 135 pivotA
rlm@64 136 pivotB)]
rlm@64 137 segmentB))
rlm@63 138
rlm@64 139 (defn eve-worm
rlm@72 140 "Create a worm-like body bound by invisible joint constraints."
rlm@64 141 []
rlm@64 142 (let [segments (worm-segments 0.2 5 0.1 0.1)]
rlm@64 143 (dorun (map (partial apply connect-at-midpoint)
rlm@64 144 (partition 2 1 segments)))
rlm@64 145 (nodify "worm" segments)))
rlm@63 146
rlm@64 147 (defn worm-pattern
rlm@64 148 "This is a simple, mindless motor control pattern that drives the
rlm@64 149 second segment of the worm's body at an offset angle with
rlm@64 150 sinusoidally varying strength."
rlm@64 151 [time]
rlm@64 152 (let [angle (* Math/PI (/ 9 20))
rlm@63 153 direction (Vector3f. 0 (Math/sin angle) (Math/cos angle))]
rlm@63 154 [Vector3f/ZERO
rlm@63 155 (.mult
rlm@63 156 direction
rlm@63 157 (float (* 2 (Math/sin (* Math/PI 2 (/ (rem time 300 ) 300))))))
rlm@63 158 Vector3f/ZERO
rlm@63 159 Vector3f/ZERO
rlm@63 160 Vector3f/ZERO]))
rlm@60 161
rlm@64 162 (defn test-motor-control
rlm@69 163 "Testing motor-control:
rlm@69 164 You should see a multi-segmented worm-like object fall onto the
rlm@64 165 table and begin writhing and moving."
rlm@60 166 []
rlm@64 167 (let [worm (eve-worm)
rlm@60 168 time (atom 0)
rlm@63 169 worm-motor-map (vector-motor-control worm)]
rlm@60 170 (world
rlm@60 171 (nodify [worm
rlm@60 172 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
rlm@60 173 :color ColorRGBA/Gray)])
rlm@60 174 standard-debug-controls
rlm@60 175 (fn [world]
rlm@60 176 (enable-debug world)
rlm@60 177 (light-up-everything world)
rlm@63 178 (comment
rlm@63 179 (com.aurellem.capture.Capture/captureVideo
rlm@63 180 world
rlm@63 181 (file-str "/home/r/proj/cortex/tmp/moving-worm")))
rlm@63 182 )
rlm@60 183
rlm@60 184 (fn [_ _]
rlm@60 185 (swap! time inc)
rlm@64 186 (Thread/sleep 20)
rlm@60 187 (dorun (worm-motor-map
rlm@60 188 (worm-pattern @time)))))))
rlm@60 189
rlm@130 190
rlm@130 191 (require 'cortex.silly)
rlm@130 192 (defn join-at-point [obj-a obj-b world-pivot]
rlm@130 193 (cortex.silly/joint-dispatch
rlm@130 194 {:type :point}
rlm@130 195 (.getControl obj-a RigidBodyControl)
rlm@130 196 (.getControl obj-b RigidBodyControl)
rlm@130 197 (cortex.silly/world-to-local obj-a world-pivot)
rlm@130 198 (cortex.silly/world-to-local obj-b world-pivot)
rlm@130 199 nil
rlm@130 200 ))
rlm@130 201
rlm@133 202 (import com.jme3.bullet.collision.PhysicsCollisionObject)
rlm@130 203
rlm@130 204 (defn blab-* []
rlm@130 205 (let [hand (box 0.5 0.2 0.2 :position (Vector3f. 0 0 0)
rlm@130 206 :mass 0 :color ColorRGBA/Green)
rlm@130 207 finger (box 0.5 0.2 0.2 :position (Vector3f. 2.4 0 0)
rlm@130 208 :mass 1 :color ColorRGBA/Red)
rlm@130 209 connection-point (Vector3f. 1.2 0 0)
rlm@130 210 root (nodify [hand finger])]
rlm@130 211
rlm@130 212 (join-at-point hand finger (Vector3f. 1.2 0 0))
rlm@130 213
rlm@130 214 (.setCollisionGroup
rlm@130 215 (.getControl hand RigidBodyControl)
rlm@130 216 PhysicsCollisionObject/COLLISION_GROUP_NONE)
rlm@130 217 (world
rlm@130 218 root
rlm@130 219 standard-debug-controls
rlm@130 220 (fn [world]
rlm@130 221 (enable-debug world)
rlm@130 222 (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
rlm@130 223 (set-gravity world Vector3f/ZERO)
rlm@130 224 )
rlm@130 225 no-op)))
rlm@131 226 (import java.awt.image.BufferedImage)
rlm@131 227
rlm@131 228 (defn draw-sprite [image sprite x y color ]
rlm@131 229 (dorun
rlm@131 230 (for [[u v] sprite]
rlm@131 231 (.setRGB image (+ u x) (+ v y) color))))
rlm@131 232
rlm@131 233 (defn view-angle
rlm@132 234 "create a debug view of an angle"
rlm@131 235 [color]
rlm@131 236 (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
rlm@131 237 previous (atom [25 25])
rlm@131 238 sprite [[0 0] [0 1]
rlm@131 239 [0 -1] [-1 0] [1 0]]]
rlm@131 240 (fn [angle]
rlm@131 241 (let [angle (float angle)]
rlm@131 242 (let [position
rlm@131 243 [(+ 25 (int (* 20 (Math/cos angle))))
rlm@131 244 (+ 25 (int (* 20(Math/sin angle))))]]
rlm@131 245 (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)
rlm@131 246 (draw-sprite image sprite (position 0) (position 1) color)
rlm@131 247 (reset! previous position))
rlm@131 248 image))))
rlm@131 249
rlm@130 250 (defn proprioception-debug-window
rlm@130 251 []
rlm@131 252 (let [yaw (view-angle 0xFF0000)
rlm@133 253 roll (view-angle 0x00FF00)
rlm@133 254 pitch (view-angle 0xFFFFFF)
rlm@131 255 v-yaw (view-image)
rlm@133 256 v-roll (view-image)
rlm@131 257 v-pitch (view-image)
rlm@131 258 ]
rlm@130 259 (fn [prop-data]
rlm@130 260 (dorun
rlm@130 261 (map
rlm@133 262 (fn [[y r p]]
rlm@131 263 (v-yaw (yaw y))
rlm@131 264 (v-roll (roll r))
rlm@131 265 (v-pitch (pitch p)))
rlm@131 266 prop-data)))))
rlm@133 267 (comment
rlm@133 268
rlm@133 269 (defn proprioception-debug-window
rlm@133 270 []
rlm@133 271 (let [time (atom 0)]
rlm@133 272 (fn [prop-data]
rlm@133 273 (if (= 0 (rem (swap! time inc) 40))
rlm@133 274 (println-repl prop-data)))))
rlm@133 275 )
rlm@133 276
rlm@131 277 (comment
rlm@131 278 (dorun
rlm@131 279 (map
rlm@131 280 (comp
rlm@131 281 println-repl
rlm@131 282 (fn [[p y r]]
rlm@131 283 (format
rlm@131 284 "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n"
rlm@131 285 p y r)))
rlm@131 286 prop-data)))
rlm@131 287
rlm@130 288
rlm@130 289
rlm@130 290
rlm@130 291
rlm@64 292 (defn test-proprioception
rlm@69 293 "Testing proprioception:
rlm@69 294 You should see two foating bars, and a printout of pitch, yaw, and
rlm@64 295 roll. Pressing key-r/key-t should move the blue bar up and down and
rlm@64 296 change only the value of pitch. key-f/key-g moves it side to side
rlm@64 297 and changes yaw. key-v/key-b will spin the blue segment clockwise
rlm@64 298 and counterclockwise, and only affect roll."
rlm@60 299 []
rlm@60 300 (let [hand (box 1 0.2 0.2 :position (Vector3f. 0 2 0)
rlm@132 301 :mass 0 :color ColorRGBA/Green :name "hand")
rlm@60 302 finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0)
rlm@132 303 :mass 1 :color ColorRGBA/Red :name "finger")
rlm@133 304 floor (box 10 10 10 :position (Vector3f. 0 -15 0)
rlm@60 305 :mass 0 :color ColorRGBA/Gray)
rlm@133 306 joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow
rlm@133 307 :position (Vector3f. 1.2 2 0)
rlm@133 308 :physical? false)
rlm@60 309
rlm@60 310 move-up? (atom false)
rlm@60 311 move-down? (atom false)
rlm@60 312 move-left? (atom false)
rlm@60 313 move-right? (atom false)
rlm@60 314 roll-left? (atom false)
rlm@60 315 roll-right? (atom false)
rlm@60 316 control (.getControl finger RigidBodyControl)
rlm@130 317 time (atom 0)
rlm@130 318 joint (join-at-point hand finger (Vector3f. 1.2 2 0 ))
rlm@133 319 creature (nodify [hand finger joint-node])
rlm@133 320 prop (joint-proprioception creature joint-node)
rlm@130 321
rlm@130 322 prop-view (proprioception-debug-window)
rlm@130 323
rlm@130 324
rlm@130 325 ]
rlm@130 326
rlm@130 327
rlm@130 328
rlm@133 329
rlm@130 330 (.setCollisionGroup
rlm@130 331 (.getControl hand RigidBodyControl)
rlm@130 332 PhysicsCollisionObject/COLLISION_GROUP_NONE)
rlm@130 333
rlm@130 334
rlm@60 335 (world
rlm@133 336 (nodify [hand finger floor joint-node])
rlm@60 337 (merge standard-debug-controls
rlm@60 338 {"key-r" (fn [_ pressed?] (reset! move-up? pressed?))
rlm@60 339 "key-t" (fn [_ pressed?] (reset! move-down? pressed?))
rlm@60 340 "key-f" (fn [_ pressed?] (reset! move-left? pressed?))
rlm@60 341 "key-g" (fn [_ pressed?] (reset! move-right? pressed?))
rlm@60 342 "key-v" (fn [_ pressed?] (reset! roll-left? pressed?))
rlm@60 343 "key-b" (fn [_ pressed?] (reset! roll-right? pressed?))})
rlm@60 344 (fn [world]
rlm@130 345 (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
rlm@61 346 (set-gravity world (Vector3f. 0 0 0))
rlm@69 347 (light-up-everything world))
rlm@60 348 (fn [_ _]
rlm@60 349 (if @move-up?
rlm@60 350 (.applyTorque control
rlm@60 351 (.mult (.getPhysicsRotation control)
rlm@61 352 (Vector3f. 0 0 10))))
rlm@60 353 (if @move-down?
rlm@60 354 (.applyTorque control
rlm@60 355 (.mult (.getPhysicsRotation control)
rlm@61 356 (Vector3f. 0 0 -10))))
rlm@60 357 (if @move-left?
rlm@60 358 (.applyTorque control
rlm@60 359 (.mult (.getPhysicsRotation control)
rlm@61 360 (Vector3f. 0 10 0))))
rlm@60 361 (if @move-right?
rlm@60 362 (.applyTorque control
rlm@60 363 (.mult (.getPhysicsRotation control)
rlm@61 364 (Vector3f. 0 -10 0))))
rlm@60 365 (if @roll-left?
rlm@60 366 (.applyTorque control
rlm@60 367 (.mult (.getPhysicsRotation control)
rlm@61 368 (Vector3f. -1 0 0))))
rlm@60 369 (if @roll-right?
rlm@60 370 (.applyTorque control
rlm@60 371 (.mult (.getPhysicsRotation control)
rlm@61 372 (Vector3f. 1 0 0))))
rlm@60 373
rlm@131 374 ;;(if (= 0 (rem (swap! time inc) 20))
rlm@133 375 (prop-view (list (prop)))))))
rlm@131 376
rlm@64 377 #+end_src
rlm@56 378
rlm@130 379 #+results: test-body
rlm@130 380 : #'cortex.test.body/test-proprioception
rlm@130 381
rlm@60 382
rlm@63 383 * COMMENT code-limbo
rlm@61 384 #+begin_src clojure
rlm@61 385 ;;(.loadModel
rlm@61 386 ;; (doto (asset-manager)
rlm@61 387 ;; (.registerLoader BlenderModelLoader (into-array String ["blend"])))
rlm@61 388 ;; "Models/person/person.blend")
rlm@61 389
rlm@64 390
rlm@64 391 (defn load-blender-model
rlm@64 392 "Load a .blend file using an asset folder relative path."
rlm@64 393 [^String model]
rlm@64 394 (.loadModel
rlm@64 395 (doto (asset-manager)
rlm@64 396 (.registerLoader BlenderModelLoader (into-array String ["blend"])))
rlm@64 397 model))
rlm@64 398
rlm@64 399
rlm@61 400 (defn view-model [^String model]
rlm@61 401 (view
rlm@61 402 (.loadModel
rlm@61 403 (doto (asset-manager)
rlm@61 404 (.registerLoader BlenderModelLoader (into-array String ["blend"])))
rlm@61 405 model)))
rlm@61 406
rlm@61 407 (defn load-blender-scene [^String model]
rlm@61 408 (.loadModel
rlm@61 409 (doto (asset-manager)
rlm@61 410 (.registerLoader BlenderLoader (into-array String ["blend"])))
rlm@61 411 model))
rlm@61 412
rlm@61 413 (defn worm
rlm@61 414 []
rlm@61 415 (.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml"))
rlm@61 416
rlm@61 417 (defn oto
rlm@61 418 []
rlm@61 419 (.loadModel (asset-manager) "Models/Oto/Oto.mesh.xml"))
rlm@61 420
rlm@61 421 (defn sinbad
rlm@61 422 []
rlm@61 423 (.loadModel (asset-manager) "Models/Sinbad/Sinbad.mesh.xml"))
rlm@61 424
rlm@61 425 (defn worm-blender
rlm@61 426 []
rlm@61 427 (first (seq (.getChildren (load-blender-model
rlm@61 428 "Models/anim2/simple-worm.blend")))))
rlm@61 429
rlm@61 430 (defn body
rlm@61 431 "given a node with a SkeletonControl, will produce a body sutiable
rlm@61 432 for AI control with movement and proprioception."
rlm@61 433 [node]
rlm@61 434 (let [skeleton-control (.getControl node SkeletonControl)
rlm@61 435 krc (KinematicRagdollControl.)]
rlm@61 436 (comment
rlm@61 437 (dorun
rlm@61 438 (map #(.addBoneName krc %)
rlm@61 439 ["mid2" "tail" "head" "mid1" "mid3" "mid4" "Dummy-Root" ""]
rlm@61 440 ;;"mid2" "mid3" "tail" "head"]
rlm@61 441 )))
rlm@61 442 (.addControl node krc)
rlm@61 443 (.setRagdollMode krc)
rlm@61 444 )
rlm@61 445 node
rlm@61 446 )
rlm@61 447 (defn show-skeleton [node]
rlm@61 448 (let [sd
rlm@61 449
rlm@61 450 (doto
rlm@61 451 (SkeletonDebugger. "aurellem-skel-debug"
rlm@61 452 (skel node))
rlm@61 453 (.setMaterial (green-x-ray)))]
rlm@61 454 (.attachChild node sd)
rlm@61 455 node))
rlm@61 456
rlm@61 457
rlm@61 458
rlm@61 459 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@61 460
rlm@61 461 ;; this could be a good way to give objects special properties like
rlm@61 462 ;; being eyes and the like
rlm@61 463
rlm@61 464 (.getUserData
rlm@61 465 (.getChild
rlm@61 466 (load-blender-model "Models/property/test.blend") 0)
rlm@61 467 "properties")
rlm@61 468
rlm@61 469 ;; the properties are saved along with the blender file.
rlm@61 470 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@61 471
rlm@61 472
rlm@61 473
rlm@61 474
rlm@61 475 (defn init-debug-skel-node
rlm@61 476 [f debug-node skeleton]
rlm@61 477 (let [bones
rlm@61 478 (map #(.getBone skeleton %)
rlm@61 479 (range (.getBoneCount skeleton)))]
rlm@61 480 (dorun (map #(.setUserControl % true) bones))
rlm@61 481 (dorun (map (fn [b]
rlm@61 482 (println (.getName b)
rlm@61 483 " -- " (f b)))
rlm@61 484 bones))
rlm@61 485 (dorun
rlm@61 486 (map #(.attachChild
rlm@61 487 debug-node
rlm@61 488 (doto
rlm@61 489 (sphere 0.1
rlm@61 490 :position (f %)
rlm@61 491 :physical? false)
rlm@61 492 (.setMaterial (green-x-ray))))
rlm@61 493 bones)))
rlm@61 494 debug-node)
rlm@61 495
rlm@61 496 (import jme3test.bullet.PhysicsTestHelper)
rlm@61 497
rlm@61 498
rlm@61 499 (defn test-zzz [the-worm world value]
rlm@61 500 (if (not value)
rlm@61 501 (let [skeleton (skel the-worm)]
rlm@61 502 (println-repl "enabling bones")
rlm@61 503 (dorun
rlm@61 504 (map
rlm@61 505 #(.setUserControl (.getBone skeleton %) true)
rlm@61 506 (range (.getBoneCount skeleton))))
rlm@61 507
rlm@61 508
rlm@61 509 (let [b (.getBone skeleton 2)]
rlm@61 510 (println-repl "moving " (.getName b))
rlm@61 511 (println-repl (.getLocalPosition b))
rlm@61 512 (.setUserTransforms b
rlm@61 513 Vector3f/UNIT_X
rlm@61 514 Quaternion/IDENTITY
rlm@61 515 ;;(doto (Quaternion.)
rlm@61 516 ;; (.fromAngles (/ Math/PI 2)
rlm@61 517 ;; 0
rlm@61 518 ;; 0
rlm@61 519
rlm@61 520 (Vector3f. 1 1 1))
rlm@61 521 )
rlm@61 522
rlm@61 523 (println-repl "hi! <3"))))
rlm@61 524
rlm@61 525
rlm@61 526 (defn test-ragdoll []
rlm@61 527
rlm@61 528 (let [the-worm
rlm@61 529
rlm@61 530 ;;(.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml")
rlm@61 531 (doto (show-skeleton (worm-blender))
rlm@61 532 (.setLocalTranslation (Vector3f. 0 10 0))
rlm@61 533 ;;(worm)
rlm@61 534 ;;(oto)
rlm@61 535 ;;(sinbad)
rlm@61 536 )
rlm@61 537 ]
rlm@61 538
rlm@61 539
rlm@61 540 (.start
rlm@61 541 (world
rlm@61 542 (doto (Node.)
rlm@61 543 (.attachChild the-worm))
rlm@61 544 {"key-return" (fire-cannon-ball)
rlm@61 545 "key-space" (partial test-zzz the-worm)
rlm@61 546 }
rlm@61 547 (fn [world]
rlm@61 548 (light-up-everything world)
rlm@61 549 (PhysicsTestHelper/createPhysicsTestWorld
rlm@61 550 (.getRootNode world)
rlm@61 551 (asset-manager)
rlm@61 552 (.getPhysicsSpace
rlm@61 553 (.getState (.getStateManager world) BulletAppState)))
rlm@61 554 (set-gravity world Vector3f/ZERO)
rlm@61 555 ;;(.setTimer world (NanoTimer.))
rlm@61 556 ;;(org.lwjgl.input.Mouse/setGrabbed false)
rlm@61 557 )
rlm@61 558 no-op
rlm@61 559 )
rlm@61 560
rlm@61 561
rlm@61 562 )))
rlm@61 563
rlm@61 564
rlm@61 565 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@61 566 ;;; here is the ragdoll stuff
rlm@61 567
rlm@61 568 (def worm-mesh (.getMesh (.getChild (worm-blender) 0)))
rlm@61 569 (def mesh worm-mesh)
rlm@61 570
rlm@61 571 (.getFloatBuffer mesh VertexBuffer$Type/Position)
rlm@61 572 (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)
rlm@61 573 (.getData (.getBuffer mesh VertexBuffer$Type/BoneIndex))
rlm@61 574
rlm@61 575
rlm@61 576 (defn position [index]
rlm@61 577 (.get
rlm@61 578 (.getFloatBuffer worm-mesh VertexBuffer$Type/Position)
rlm@61 579 index))
rlm@61 580
rlm@61 581 (defn bones [index]
rlm@61 582 (.get
rlm@61 583 (.getData (.getBuffer mesh VertexBuffer$Type/BoneIndex))
rlm@61 584 index))
rlm@61 585
rlm@61 586 (defn bone-weights [index]
rlm@61 587 (.get
rlm@61 588 (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)
rlm@61 589 index))
rlm@61 590
rlm@61 591
rlm@61 592
rlm@61 593 (defn vertex-bones [vertex]
rlm@61 594 (vec (map (comp int bones) (range (* vertex 4) (+ (* vertex 4) 4)))))
rlm@61 595
rlm@61 596 (defn vertex-weights [vertex]
rlm@61 597 (vec (map (comp float bone-weights) (range (* vertex 4) (+ (* vertex 4) 4)))))
rlm@61 598
rlm@61 599 (defn vertex-position [index]
rlm@61 600 (let [offset (* index 3)]
rlm@61 601 (Vector3f. (position offset)
rlm@61 602 (position (inc offset))
rlm@61 603 (position (inc(inc offset))))))
rlm@61 604
rlm@61 605 (def vertex-info (juxt vertex-position vertex-bones vertex-weights))
rlm@61 606
rlm@61 607 (defn bone-control-color [index]
rlm@61 608 (get {[1 0 0 0] ColorRGBA/Red
rlm@61 609 [1 2 0 0] ColorRGBA/Magenta
rlm@61 610 [2 0 0 0] ColorRGBA/Blue}
rlm@61 611 (vertex-bones index)
rlm@61 612 ColorRGBA/White))
rlm@61 613
rlm@61 614 (defn influence-color [index bone-num]
rlm@61 615 (get
rlm@61 616 {(float 0) ColorRGBA/Blue
rlm@61 617 (float 0.5) ColorRGBA/Green
rlm@61 618 (float 1) ColorRGBA/Red}
rlm@61 619 ;; find the weight of the desired bone
rlm@61 620 ((zipmap (vertex-bones index)(vertex-weights index))
rlm@61 621 bone-num)
rlm@61 622 ColorRGBA/Blue))
rlm@61 623
rlm@61 624 (def worm-vertices (set (map vertex-info (range 60))))
rlm@61 625
rlm@61 626
rlm@61 627 (defn test-info []
rlm@61 628 (let [points (Node.)]
rlm@61 629 (dorun
rlm@61 630 (map #(.attachChild points %)
rlm@61 631 (map #(sphere 0.01
rlm@61 632 :position (vertex-position %)
rlm@61 633 :color (influence-color % 1)
rlm@61 634 :physical? false)
rlm@61 635 (range 60))))
rlm@61 636 (view points)))
rlm@61 637
rlm@61 638
rlm@61 639 (defrecord JointControl [joint physics-space]
rlm@61 640 PhysicsControl
rlm@61 641 (setPhysicsSpace [this space]
rlm@61 642 (dosync
rlm@61 643 (ref-set (:physics-space this) space))
rlm@61 644 (.addJoint space (:joint this)))
rlm@61 645 (update [this tpf])
rlm@61 646 (setSpatial [this spatial])
rlm@61 647 (render [this rm vp])
rlm@61 648 (getPhysicsSpace [this] (deref (:physics-space this)))
rlm@61 649 (isEnabled [this] true)
rlm@61 650 (setEnabled [this state]))
rlm@61 651
rlm@61 652 (defn add-joint
rlm@61 653 "Add a joint to a particular object. When the object is added to the
rlm@61 654 PhysicsSpace of a simulation, the joint will also be added"
rlm@61 655 [object joint]
rlm@61 656 (let [control (JointControl. joint (ref nil))]
rlm@61 657 (.addControl object control))
rlm@61 658 object)
rlm@61 659
rlm@61 660
rlm@61 661 (defn hinge-world
rlm@61 662 []
rlm@61 663 (let [sphere1 (sphere)
rlm@61 664 sphere2 (sphere 1 :position (Vector3f. 3 3 3))
rlm@61 665 joint (Point2PointJoint.
rlm@61 666 (.getControl sphere1 RigidBodyControl)
rlm@61 667 (.getControl sphere2 RigidBodyControl)
rlm@61 668 Vector3f/ZERO (Vector3f. 3 3 3))]
rlm@61 669 (add-joint sphere1 joint)
rlm@61 670 (doto (Node. "hinge-world")
rlm@61 671 (.attachChild sphere1)
rlm@61 672 (.attachChild sphere2))))
rlm@61 673
rlm@61 674
rlm@61 675 (defn test-joint []
rlm@61 676 (view (hinge-world)))
rlm@61 677
rlm@61 678 ;; (defn copier-gen []
rlm@61 679 ;; (let [count (atom 0)]
rlm@61 680 ;; (fn [in]
rlm@61 681 ;; (swap! count inc)
rlm@61 682 ;; (clojure.contrib.duck-streams/copy
rlm@61 683 ;; in (File. (str "/home/r/tmp/mao-test/clojure-images/"
rlm@61 684 ;; ;;/home/r/tmp/mao-test/clojure-images
rlm@61 685 ;; (format "%08d.png" @count)))))))
rlm@61 686 ;; (defn decrease-framerate []
rlm@61 687 ;; (map
rlm@61 688 ;; (copier-gen)
rlm@61 689 ;; (sort
rlm@61 690 ;; (map first
rlm@61 691 ;; (partition
rlm@61 692 ;; 4
rlm@61 693 ;; (filter #(re-matches #".*.png$" (.getCanonicalPath %))
rlm@61 694 ;; (file-seq
rlm@61 695 ;; (file-str
rlm@61 696 ;; "/home/r/media/anime/mao-temp/images"))))))))
rlm@61 697
rlm@61 698
rlm@61 699
rlm@61 700 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
rlm@61 701
rlm@61 702 (defn proprioception
rlm@61 703 "Create a proprioception map that reports the rotations of the
rlm@61 704 various limbs of the creature's body"
rlm@61 705 [creature]
rlm@61 706 [#^Node creature]
rlm@61 707 (let [
rlm@61 708 nodes (node-seq creature)
rlm@61 709 joints
rlm@61 710 (map
rlm@61 711 :joint
rlm@61 712 (filter
rlm@61 713 #(isa? (class %) JointControl)
rlm@61 714 (reduce
rlm@61 715 concat
rlm@61 716 (map (fn [node]
rlm@61 717 (map (fn [num] (.getControl node num))
rlm@61 718 (range (.getNumControls node))))
rlm@61 719 nodes))))]
rlm@61 720 (fn []
rlm@61 721 (reduce concat (map relative-positions (list (first joints)))))))
rlm@61 722
rlm@61 723
rlm@63 724 (defn skel [node]
rlm@63 725 (doto
rlm@63 726 (.getSkeleton
rlm@63 727 (.getControl node SkeletonControl))
rlm@63 728 ;; this is necessary to force the skeleton to have accurate world
rlm@63 729 ;; transforms before it is rendered to the screen.
rlm@63 730 (.resetAndUpdate)))
rlm@63 731
rlm@63 732 (defn green-x-ray []
rlm@63 733 (doto (Material. (asset-manager)
rlm@63 734 "Common/MatDefs/Misc/Unshaded.j3md")
rlm@63 735 (.setColor "Color" ColorRGBA/Green)
rlm@63 736 (-> (.getAdditionalRenderState)
rlm@63 737 (.setDepthTest false))))
rlm@63 738
rlm@63 739 (defn test-worm []
rlm@63 740 (.start
rlm@63 741 (world
rlm@63 742 (doto (Node.)
rlm@63 743 ;;(.attachChild (point-worm))
rlm@63 744 (.attachChild (load-blender-model
rlm@63 745 "Models/anim2/joint-worm.blend"))
rlm@63 746
rlm@63 747 (.attachChild (box 10 1 10
rlm@63 748 :position (Vector3f. 0 -2 0) :mass 0
rlm@63 749 :color (ColorRGBA/Gray))))
rlm@63 750 {
rlm@63 751 "key-space" (fire-cannon-ball)
rlm@63 752 }
rlm@63 753 (fn [world]
rlm@63 754 (enable-debug world)
rlm@63 755 (light-up-everything world)
rlm@63 756 ;;(.setTimer world (NanoTimer.))
rlm@63 757 )
rlm@63 758 no-op)))
rlm@63 759
rlm@63 760
rlm@63 761
rlm@63 762 ;; defunct movement stuff
rlm@63 763 (defn torque-controls [control]
rlm@63 764 (let [torques
rlm@63 765 (concat
rlm@63 766 (map #(Vector3f. 0 (Math/sin %) (Math/cos %))
rlm@63 767 (range 0 (* Math/PI 2) (/ (* Math/PI 2) 20)))
rlm@63 768 [Vector3f/UNIT_X])]
rlm@63 769 (map (fn [torque-axis]
rlm@63 770 (fn [torque]
rlm@63 771 (.applyTorque
rlm@63 772 control
rlm@63 773 (.mult (.mult (.getPhysicsRotation control)
rlm@63 774 torque-axis)
rlm@63 775 (float
rlm@63 776 (* (.getMass control) torque))))))
rlm@63 777 torques)))
rlm@63 778
rlm@63 779 (defn motor-map
rlm@63 780 "Take a creature and generate a function that will enable fine
rlm@63 781 grained control over all the creature's limbs."
rlm@63 782 [#^Node creature]
rlm@63 783 (let [controls (keep #(.getControl % RigidBodyControl)
rlm@63 784 (node-seq creature))
rlm@63 785 limb-controls (reduce concat (map torque-controls controls))
rlm@63 786 body-control (partial map #(%1 %2) limb-controls)]
rlm@63 787 body-control))
rlm@63 788
rlm@63 789 (defn test-motor-map
rlm@63 790 "see how torque works."
rlm@63 791 []
rlm@63 792 (let [finger (box 3 0.5 0.5 :position (Vector3f. 0 2 0)
rlm@63 793 :mass 1 :color ColorRGBA/Green)
rlm@63 794 motor-map (motor-map finger)]
rlm@63 795 (world
rlm@63 796 (nodify [finger
rlm@63 797 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0
rlm@63 798 :color ColorRGBA/Gray)])
rlm@63 799 standard-debug-controls
rlm@63 800 (fn [world]
rlm@63 801 (set-gravity world Vector3f/ZERO)
rlm@63 802 (light-up-everything world)
rlm@63 803 (.setTimer world (NanoTimer.)))
rlm@63 804 (fn [_ _]
rlm@63 805 (dorun (motor-map [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0]))))))
rlm@61 806 #+end_src
rlm@0 807
rlm@0 808
rlm@0 809
rlm@0 810
rlm@0 811
rlm@0 812
rlm@0 813
rlm@73 814 * COMMENT generate Source
rlm@44 815 #+begin_src clojure :tangle ../src/cortex/body.clj
rlm@64 816 <<proprioception>>
rlm@64 817 <<motor-control>>
rlm@0 818 #+end_src
rlm@64 819
rlm@69 820 #+begin_src clojure :tangle ../src/cortex/test/body.clj
rlm@64 821 <<test-body>>
rlm@64 822 #+end_src
rlm@64 823
rlm@64 824
rlm@0 825