Mercurial > cortex

annotate org/body.org @ 137:39c89ae5c7d0

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