Mercurial > cortex

annotate org/body.org @ 131:e98850b83c2c

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