Mercurial > cortex

annotate org/body.org @ 145:7a49b81ca1bf

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