Mercurial > cortex

annotate org/body.org @ 133:2ed7e60d3821

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