Mercurial > cortex
view org/body.org @ 64:ab1fee4280c3
divided body into test suite and core functions
author | Robert McIntyre <rlm@mit.edu> |
---|---|
date | Wed, 30 Nov 2011 20:42:07 -0700 |
parents | 7f2653ad3199 |
children | 4b5f00110d8c |
line wrap: on
line source
1 #+title: The BODY!!!2 #+author: Robert McIntyre3 #+email: rlm@mit.edu4 #+description: Simulating a body (movement, touch, propioception) in jMonkeyEngine3.5 #+SETUPFILE: ../../aurellem/org/setup.org6 #+INCLUDE: ../../aurellem/org/level-0.org10 * Proprioception11 #+srcname: proprioception12 #+begin_src clojure13 (ns cortex.body14 (:use (cortex world util))15 (:import16 com.jme3.math.Vector3f17 com.jme3.math.Quaternion18 com.jme3.math.Vector2f19 com.jme3.math.Matrix3f20 com.jme3.bullet.control.RigidBodyControl))22 (defn any-orthogonal23 "Generate an arbitray (but stable) orthogonal vector to a given24 vector."25 [vector]26 (let [x (.getX vector)27 y (.getY vector)28 z (.getZ vector)]29 (cond30 (not= x (float 0)) (Vector3f. (- z) 0 x)31 (not= y (float 0)) (Vector3f. 0 (- z) y)32 (not= z (float 0)) (Vector3f. 0 (- z) y)33 true Vector3f/ZERO)))35 (defn project-quaternion36 "From http://stackoverflow.com/questions/3684269/37 component-of-a-quaternion-rotation-around-an-axis.39 Determine the amount of rotation a quaternion will40 cause about a given axis."41 [#^Quaternion q #^Vector3f axis]42 (let [basis-1 (any-orthogonal axis)43 basis-2 (.cross axis basis-1)44 rotated (.mult q basis-1)45 alpha (.dot basis-1 (.project rotated basis-1))46 beta (.dot basis-2 (.project rotated basis-2))]47 (Math/atan2 beta alpha)))49 (defn joint-proprioception50 "Relative position information for a two-part system connected by a51 joint. Gives the pitch, yaw, and roll of the 'B' object relative to52 the 'A' object, as determined by the joint."53 [joint]54 (let [object-a (.getUserObject (.getBodyA joint))55 object-b (.getUserObject (.getBodyB joint))56 arm-a57 (.normalize58 (.subtract59 (.localToWorld object-a (.getPivotA joint) nil)60 (.getWorldTranslation object-a)))61 rotate-a62 (doto (Matrix3f.)63 (.fromStartEndVectors arm-a Vector3f/UNIT_X))64 arm-b65 (.mult66 rotate-a67 (.normalize68 (.subtract69 (.localToWorld object-b (.getPivotB joint) nil)70 (.getWorldTranslation object-b))))71 pitch72 (.angleBetween73 (.normalize (Vector2f. (.getX arm-b) (.getY arm-b)))74 (Vector2f. 1 0))75 yaw76 (.angleBetween77 (.normalize (Vector2f. (.getX arm-b) (.getZ arm-b)))78 (Vector2f. 1 0))80 roll81 (project-quaternion82 (.mult83 (.getLocalRotation object-b)84 (doto (Quaternion.)85 (.fromRotationMatrix rotate-a)))86 arm-b)]87 [pitch yaw roll]))89 (defn proprioception90 "Create a function that provides proprioceptive information about an91 entire body."92 [body]93 ;; extract the body's joints94 (let [joints95 (distinct96 (reduce97 concat98 (map #(.getJoints %)99 (keep100 #(.getControl % RigidBodyControl)101 (node-seq body)))))]102 (fn []103 (map joint-proprioception joints))))105 #+end_src107 #+srcname: motor-control108 #+begin_src clojure109 (in-ns 'cortex.body)111 ;; surprisingly enough, terristerial creatures only move by using112 ;; torque applied about their joints. There's not a single straight113 ;; line of force in the human body at all! (A straight line of force114 ;; would correspond to some sort of jet or rocket propulseion.)116 (defn vector-motor-control117 "Create a function that accepts a sequence of Vector3f objects that118 describe the torque to be applied to each part of the body."119 [body]120 (let [nodes (node-seq body)121 controls (keep #(.getControl % RigidBodyControl) nodes)]122 (fn [torques]123 (map #(.applyTorque %1 %2)124 controls torques))))125 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;126 #+end_src128 ## note -- might want to add a lower dimensional, discrete version of129 ## this if it proves useful from a x-modal clustering perspective.131 * Examples133 #+srcname: test-body134 #+begin_src clojure135 (ns test.body136 (:use (cortex world util body))137 (:import138 com.jme3.math.Vector3f139 com.jme3.math.ColorRGBA140 com.jme3.bullet.joints.Point2PointJoint141 com.jme3.bullet.control.RigidBodyControl142 com.jme3.system.NanoTimer))144 (defn worm-segments145 "Create multiple evenly spaced box segments. They're fabulous!"146 [segment-length num-segments interstitial-space radius]147 (letfn [(nth-segment148 [n]149 (box segment-length radius radius :mass 0.1150 :position151 (Vector3f.152 (* 2 n (+ interstitial-space segment-length)) 0 0)153 :name (str "worm-segment" n)154 :color (ColorRGBA/randomColor)))]155 (map nth-segment (range num-segments))))157 (defn connect-at-midpoint158 "Connect two physics objects with a Point2Point joint constraint at159 the point equidistant from both objects' centers."160 [segmentA segmentB]161 (let [centerA (.getWorldTranslation segmentA)162 centerB (.getWorldTranslation segmentB)163 midpoint (.mult (.add centerA centerB) (float 0.5))164 pivotA (.subtract midpoint centerA)165 pivotB (.subtract midpoint centerB)167 ;; A side-effect of creating a joint registers168 ;; it with both physics objects which in turn169 ;; will register the joint with the physics system170 ;; when the simulation is started.171 joint (Point2PointJoint.172 (.getControl segmentA RigidBodyControl)173 (.getControl segmentB RigidBodyControl)174 pivotA175 pivotB)]176 segmentB))178 (defn eve-worm179 "Create a worm body bound by invisible joint constraints."180 []181 (let [segments (worm-segments 0.2 5 0.1 0.1)]182 (dorun (map (partial apply connect-at-midpoint)183 (partition 2 1 segments)))184 (nodify "worm" segments)))186 (defn worm-pattern187 "This is a simple, mindless motor control pattern that drives the188 second segment of the worm's body at an offset angle with189 sinusoidally varying strength."190 [time]191 (let [angle (* Math/PI (/ 9 20))192 direction (Vector3f. 0 (Math/sin angle) (Math/cos angle))]193 [Vector3f/ZERO194 (.mult195 direction196 (float (* 2 (Math/sin (* Math/PI 2 (/ (rem time 300 ) 300))))))197 Vector3f/ZERO198 Vector3f/ZERO199 Vector3f/ZERO]))201 (defn test-motor-control202 "You should see a multi-segmented worm-like object fall onto the203 table and begin writhing and moving."204 []205 (let [worm (eve-worm)206 time (atom 0)207 worm-motor-map (vector-motor-control worm)]208 (world209 (nodify [worm210 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0211 :color ColorRGBA/Gray)])212 standard-debug-controls213 (fn [world]214 (enable-debug world)215 (light-up-everything world)216 (comment217 (com.aurellem.capture.Capture/captureVideo218 world219 (file-str "/home/r/proj/cortex/tmp/moving-worm")))220 )222 (fn [_ _]223 (swap! time inc)224 (Thread/sleep 20)225 (dorun (worm-motor-map226 (worm-pattern @time)))))))228 (defn test-proprioception229 "You should see two foating bars, and a printout of pitch, yaw, and230 roll. Pressing key-r/key-t should move the blue bar up and down and231 change only the value of pitch. key-f/key-g moves it side to side232 and changes yaw. key-v/key-b will spin the blue segment clockwise233 and counterclockwise, and only affect roll."234 []235 (let [hand (box 1 0.2 0.2 :position (Vector3f. 0 2 0)236 :mass 0 :color ColorRGBA/Green)237 finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0)238 :mass 1 :color (ColorRGBA. 0.20 0.40 0.99 1.0))239 floor (box 10 0.5 10 :position (Vector3f. 0 -5 0)240 :mass 0 :color ColorRGBA/Gray)242 move-up? (atom false)243 move-down? (atom false)244 move-left? (atom false)245 move-right? (atom false)246 roll-left? (atom false)247 roll-right? (atom false)248 control (.getControl finger RigidBodyControl)249 joint250 (doto251 (Point2PointJoint.252 (.getControl hand RigidBodyControl)253 control254 (Vector3f. 1.2 0 0)255 (Vector3f. -1.2 0 0 ))256 (.setCollisionBetweenLinkedBodys false))257 time (atom 0)]258 (world259 (nodify [hand finger floor])260 (merge standard-debug-controls261 {"key-r" (fn [_ pressed?] (reset! move-up? pressed?))262 "key-t" (fn [_ pressed?] (reset! move-down? pressed?))263 "key-f" (fn [_ pressed?] (reset! move-left? pressed?))264 "key-g" (fn [_ pressed?] (reset! move-right? pressed?))265 "key-v" (fn [_ pressed?] (reset! roll-left? pressed?))266 "key-b" (fn [_ pressed?] (reset! roll-right? pressed?))})267 (fn [world]268 (set-gravity world (Vector3f. 0 0 0))269 (.setMoveSpeed (.getFlyByCamera world) 50)270 (.setRotationSpeed (.getFlyByCamera world) 50)271 (light-up-everything world)272 (.setTimer world (NanoTimer.))273 )274 (fn [_ _]275 (if @move-up?276 (.applyTorque control277 (.mult (.getPhysicsRotation control)278 (Vector3f. 0 0 10))))279 (if @move-down?280 (.applyTorque control281 (.mult (.getPhysicsRotation control)282 (Vector3f. 0 0 -10))))283 (if @move-left?284 (.applyTorque control285 (.mult (.getPhysicsRotation control)286 (Vector3f. 0 10 0))))287 (if @move-right?288 (.applyTorque control289 (.mult (.getPhysicsRotation control)290 (Vector3f. 0 -10 0))))291 (if @roll-left?292 (.applyTorque control293 (.mult (.getPhysicsRotation control)294 (Vector3f. -1 0 0))))295 (if @roll-right?296 (.applyTorque control297 (.mult (.getPhysicsRotation control)298 (Vector3f. 1 0 0))))300 (if (= 0 (rem (swap! time inc) 2000))301 (do303 (apply304 (comp305 println-repl306 #(format "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n" %1 %2 %3))307 (joint-proprioception joint))))))))308 #+end_src310 #+results: test-body311 : #'test.body/test-proprioception315 * COMMENT code-limbo316 #+begin_src clojure317 ;;(.loadModel318 ;; (doto (asset-manager)319 ;; (.registerLoader BlenderModelLoader (into-array String ["blend"])))320 ;; "Models/person/person.blend")323 (defn load-blender-model324 "Load a .blend file using an asset folder relative path."325 [^String model]326 (.loadModel327 (doto (asset-manager)328 (.registerLoader BlenderModelLoader (into-array String ["blend"])))329 model))332 (defn view-model [^String model]333 (view334 (.loadModel335 (doto (asset-manager)336 (.registerLoader BlenderModelLoader (into-array String ["blend"])))337 model)))339 (defn load-blender-scene [^String model]340 (.loadModel341 (doto (asset-manager)342 (.registerLoader BlenderLoader (into-array String ["blend"])))343 model))345 (defn worm346 []347 (.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml"))349 (defn oto350 []351 (.loadModel (asset-manager) "Models/Oto/Oto.mesh.xml"))353 (defn sinbad354 []355 (.loadModel (asset-manager) "Models/Sinbad/Sinbad.mesh.xml"))357 (defn worm-blender358 []359 (first (seq (.getChildren (load-blender-model360 "Models/anim2/simple-worm.blend")))))362 (defn body363 "given a node with a SkeletonControl, will produce a body sutiable364 for AI control with movement and proprioception."365 [node]366 (let [skeleton-control (.getControl node SkeletonControl)367 krc (KinematicRagdollControl.)]368 (comment369 (dorun370 (map #(.addBoneName krc %)371 ["mid2" "tail" "head" "mid1" "mid3" "mid4" "Dummy-Root" ""]372 ;;"mid2" "mid3" "tail" "head"]373 )))374 (.addControl node krc)375 (.setRagdollMode krc)376 )377 node378 )379 (defn show-skeleton [node]380 (let [sd382 (doto383 (SkeletonDebugger. "aurellem-skel-debug"384 (skel node))385 (.setMaterial (green-x-ray)))]386 (.attachChild node sd)387 node))391 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;393 ;; this could be a good way to give objects special properties like394 ;; being eyes and the like396 (.getUserData397 (.getChild398 (load-blender-model "Models/property/test.blend") 0)399 "properties")401 ;; the properties are saved along with the blender file.402 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;407 (defn init-debug-skel-node408 [f debug-node skeleton]409 (let [bones410 (map #(.getBone skeleton %)411 (range (.getBoneCount skeleton)))]412 (dorun (map #(.setUserControl % true) bones))413 (dorun (map (fn [b]414 (println (.getName b)415 " -- " (f b)))416 bones))417 (dorun418 (map #(.attachChild419 debug-node420 (doto421 (sphere 0.1422 :position (f %)423 :physical? false)424 (.setMaterial (green-x-ray))))425 bones)))426 debug-node)428 (import jme3test.bullet.PhysicsTestHelper)431 (defn test-zzz [the-worm world value]432 (if (not value)433 (let [skeleton (skel the-worm)]434 (println-repl "enabling bones")435 (dorun436 (map437 #(.setUserControl (.getBone skeleton %) true)438 (range (.getBoneCount skeleton))))441 (let [b (.getBone skeleton 2)]442 (println-repl "moving " (.getName b))443 (println-repl (.getLocalPosition b))444 (.setUserTransforms b445 Vector3f/UNIT_X446 Quaternion/IDENTITY447 ;;(doto (Quaternion.)448 ;; (.fromAngles (/ Math/PI 2)449 ;; 0450 ;; 0452 (Vector3f. 1 1 1))453 )455 (println-repl "hi! <3"))))458 (defn test-ragdoll []460 (let [the-worm462 ;;(.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml")463 (doto (show-skeleton (worm-blender))464 (.setLocalTranslation (Vector3f. 0 10 0))465 ;;(worm)466 ;;(oto)467 ;;(sinbad)468 )469 ]472 (.start473 (world474 (doto (Node.)475 (.attachChild the-worm))476 {"key-return" (fire-cannon-ball)477 "key-space" (partial test-zzz the-worm)478 }479 (fn [world]480 (light-up-everything world)481 (PhysicsTestHelper/createPhysicsTestWorld482 (.getRootNode world)483 (asset-manager)484 (.getPhysicsSpace485 (.getState (.getStateManager world) BulletAppState)))486 (set-gravity world Vector3f/ZERO)487 ;;(.setTimer world (NanoTimer.))488 ;;(org.lwjgl.input.Mouse/setGrabbed false)489 )490 no-op491 )494 )))497 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;498 ;;; here is the ragdoll stuff500 (def worm-mesh (.getMesh (.getChild (worm-blender) 0)))501 (def mesh worm-mesh)503 (.getFloatBuffer mesh VertexBuffer$Type/Position)504 (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)505 (.getData (.getBuffer mesh VertexBuffer$Type/BoneIndex))508 (defn position [index]509 (.get510 (.getFloatBuffer worm-mesh VertexBuffer$Type/Position)511 index))513 (defn bones [index]514 (.get515 (.getData (.getBuffer mesh VertexBuffer$Type/BoneIndex))516 index))518 (defn bone-weights [index]519 (.get520 (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)521 index))525 (defn vertex-bones [vertex]526 (vec (map (comp int bones) (range (* vertex 4) (+ (* vertex 4) 4)))))528 (defn vertex-weights [vertex]529 (vec (map (comp float bone-weights) (range (* vertex 4) (+ (* vertex 4) 4)))))531 (defn vertex-position [index]532 (let [offset (* index 3)]533 (Vector3f. (position offset)534 (position (inc offset))535 (position (inc(inc offset))))))537 (def vertex-info (juxt vertex-position vertex-bones vertex-weights))539 (defn bone-control-color [index]540 (get {[1 0 0 0] ColorRGBA/Red541 [1 2 0 0] ColorRGBA/Magenta542 [2 0 0 0] ColorRGBA/Blue}543 (vertex-bones index)544 ColorRGBA/White))546 (defn influence-color [index bone-num]547 (get548 {(float 0) ColorRGBA/Blue549 (float 0.5) ColorRGBA/Green550 (float 1) ColorRGBA/Red}551 ;; find the weight of the desired bone552 ((zipmap (vertex-bones index)(vertex-weights index))553 bone-num)554 ColorRGBA/Blue))556 (def worm-vertices (set (map vertex-info (range 60))))559 (defn test-info []560 (let [points (Node.)]561 (dorun562 (map #(.attachChild points %)563 (map #(sphere 0.01564 :position (vertex-position %)565 :color (influence-color % 1)566 :physical? false)567 (range 60))))568 (view points)))571 (defrecord JointControl [joint physics-space]572 PhysicsControl573 (setPhysicsSpace [this space]574 (dosync575 (ref-set (:physics-space this) space))576 (.addJoint space (:joint this)))577 (update [this tpf])578 (setSpatial [this spatial])579 (render [this rm vp])580 (getPhysicsSpace [this] (deref (:physics-space this)))581 (isEnabled [this] true)582 (setEnabled [this state]))584 (defn add-joint585 "Add a joint to a particular object. When the object is added to the586 PhysicsSpace of a simulation, the joint will also be added"587 [object joint]588 (let [control (JointControl. joint (ref nil))]589 (.addControl object control))590 object)593 (defn hinge-world594 []595 (let [sphere1 (sphere)596 sphere2 (sphere 1 :position (Vector3f. 3 3 3))597 joint (Point2PointJoint.598 (.getControl sphere1 RigidBodyControl)599 (.getControl sphere2 RigidBodyControl)600 Vector3f/ZERO (Vector3f. 3 3 3))]601 (add-joint sphere1 joint)602 (doto (Node. "hinge-world")603 (.attachChild sphere1)604 (.attachChild sphere2))))607 (defn test-joint []608 (view (hinge-world)))610 ;; (defn copier-gen []611 ;; (let [count (atom 0)]612 ;; (fn [in]613 ;; (swap! count inc)614 ;; (clojure.contrib.duck-streams/copy615 ;; in (File. (str "/home/r/tmp/mao-test/clojure-images/"616 ;; ;;/home/r/tmp/mao-test/clojure-images617 ;; (format "%08d.png" @count)))))))618 ;; (defn decrease-framerate []619 ;; (map620 ;; (copier-gen)621 ;; (sort622 ;; (map first623 ;; (partition624 ;; 4625 ;; (filter #(re-matches #".*.png$" (.getCanonicalPath %))626 ;; (file-seq627 ;; (file-str628 ;; "/home/r/media/anime/mao-temp/images"))))))))632 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;634 (defn proprioception635 "Create a proprioception map that reports the rotations of the636 various limbs of the creature's body"637 [creature]638 [#^Node creature]639 (let [640 nodes (node-seq creature)641 joints642 (map643 :joint644 (filter645 #(isa? (class %) JointControl)646 (reduce647 concat648 (map (fn [node]649 (map (fn [num] (.getControl node num))650 (range (.getNumControls node))))651 nodes))))]652 (fn []653 (reduce concat (map relative-positions (list (first joints)))))))656 (defn skel [node]657 (doto658 (.getSkeleton659 (.getControl node SkeletonControl))660 ;; this is necessary to force the skeleton to have accurate world661 ;; transforms before it is rendered to the screen.662 (.resetAndUpdate)))664 (defn green-x-ray []665 (doto (Material. (asset-manager)666 "Common/MatDefs/Misc/Unshaded.j3md")667 (.setColor "Color" ColorRGBA/Green)668 (-> (.getAdditionalRenderState)669 (.setDepthTest false))))671 (defn test-worm []672 (.start673 (world674 (doto (Node.)675 ;;(.attachChild (point-worm))676 (.attachChild (load-blender-model677 "Models/anim2/joint-worm.blend"))679 (.attachChild (box 10 1 10680 :position (Vector3f. 0 -2 0) :mass 0681 :color (ColorRGBA/Gray))))682 {683 "key-space" (fire-cannon-ball)684 }685 (fn [world]686 (enable-debug world)687 (light-up-everything world)688 ;;(.setTimer world (NanoTimer.))689 )690 no-op)))694 ;; defunct movement stuff695 (defn torque-controls [control]696 (let [torques697 (concat698 (map #(Vector3f. 0 (Math/sin %) (Math/cos %))699 (range 0 (* Math/PI 2) (/ (* Math/PI 2) 20)))700 [Vector3f/UNIT_X])]701 (map (fn [torque-axis]702 (fn [torque]703 (.applyTorque704 control705 (.mult (.mult (.getPhysicsRotation control)706 torque-axis)707 (float708 (* (.getMass control) torque))))))709 torques)))711 (defn motor-map712 "Take a creature and generate a function that will enable fine713 grained control over all the creature's limbs."714 [#^Node creature]715 (let [controls (keep #(.getControl % RigidBodyControl)716 (node-seq creature))717 limb-controls (reduce concat (map torque-controls controls))718 body-control (partial map #(%1 %2) limb-controls)]719 body-control))721 (defn test-motor-map722 "see how torque works."723 []724 (let [finger (box 3 0.5 0.5 :position (Vector3f. 0 2 0)725 :mass 1 :color ColorRGBA/Green)726 motor-map (motor-map finger)]727 (world728 (nodify [finger729 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0730 :color ColorRGBA/Gray)])731 standard-debug-controls732 (fn [world]733 (set-gravity world Vector3f/ZERO)734 (light-up-everything world)735 (.setTimer world (NanoTimer.)))736 (fn [_ _]737 (dorun (motor-map [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0]))))))740 #+end_src748 * COMMENT generate Source.749 #+begin_src clojure :tangle ../src/cortex/body.clj750 <<proprioception>>751 <<motor-control>>752 #+end_src754 #+begin_src clojure :tangle ../src/test/body.clj755 <<test-body>>756 #+end_src