Mercurial > cortex
view org/body.org @ 85:00ab1f10266f
creatig cone joints for hand
author | Robert McIntyre <rlm@mit.edu> |
---|---|
date | Sat, 07 Jan 2012 01:32:15 -0700 |
parents | fb810a2c50c2 |
children | b26017d1fe9a |
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.org8 * Proprioception9 #+name: proprioception10 #+begin_src clojure11 (ns cortex.body12 (:use (cortex world util))13 (:import14 com.jme3.math.Vector3f15 com.jme3.math.Quaternion16 com.jme3.math.Vector2f17 com.jme3.math.Matrix3f18 com.jme3.bullet.control.RigidBodyControl))20 (defn any-orthogonal21 "Generate an arbitray (but stable) orthogonal vector to a given22 vector."23 [vector]24 (let [x (.getX vector)25 y (.getY vector)26 z (.getZ vector)]27 (cond28 (not= x (float 0)) (Vector3f. (- z) 0 x)29 (not= y (float 0)) (Vector3f. 0 (- z) y)30 (not= z (float 0)) (Vector3f. 0 (- z) y)31 true Vector3f/ZERO)))33 (defn project-quaternion34 "From http://stackoverflow.com/questions/3684269/35 component-of-a-quaternion-rotation-around-an-axis.37 Determine the amount of rotation a quaternion will38 cause about a given axis."39 [#^Quaternion q #^Vector3f axis]40 (let [basis-1 (any-orthogonal axis)41 basis-2 (.cross axis basis-1)42 rotated (.mult q basis-1)43 alpha (.dot basis-1 (.project rotated basis-1))44 beta (.dot basis-2 (.project rotated basis-2))]45 (Math/atan2 beta alpha)))47 (defn joint-proprioception48 "Relative position information for a two-part system connected by a49 joint. Gives the pitch, yaw, and roll of the 'B' object relative to50 the 'A' object, as determined by the joint."51 [joint]52 (let [object-a (.getUserObject (.getBodyA joint))53 object-b (.getUserObject (.getBodyB joint))54 arm-a55 (.normalize56 (.subtract57 (.localToWorld object-a (.getPivotA joint) nil)58 (.getWorldTranslation object-a)))59 rotate-a60 (doto (Matrix3f.)61 (.fromStartEndVectors arm-a Vector3f/UNIT_X))62 arm-b63 (.mult64 rotate-a65 (.normalize66 (.subtract67 (.localToWorld object-b (.getPivotB joint) nil)68 (.getWorldTranslation object-b))))69 pitch70 (.angleBetween71 (.normalize (Vector2f. (.getX arm-b) (.getY arm-b)))72 (Vector2f. 1 0))73 yaw74 (.angleBetween75 (.normalize (Vector2f. (.getX arm-b) (.getZ arm-b)))76 (Vector2f. 1 0))78 roll79 (project-quaternion80 (.mult81 (.getLocalRotation object-b)82 (doto (Quaternion.)83 (.fromRotationMatrix rotate-a)))84 arm-b)]85 [pitch yaw roll]))87 (defn proprioception88 "Create a function that provides proprioceptive information about an89 entire body."90 [body]91 ;; extract the body's joints92 (let [joints93 (distinct94 (reduce95 concat96 (map #(.getJoints %)97 (keep98 #(.getControl % RigidBodyControl)99 (node-seq body)))))]100 (fn []101 (map joint-proprioception joints))))103 #+end_src105 * Motor Control106 #+name: motor-control107 #+begin_src clojure108 (in-ns 'cortex.body)110 ;; surprisingly enough, terristerial creatures only move by using111 ;; torque applied about their joints. There's not a single straight112 ;; line of force in the human body at all! (A straight line of force113 ;; would correspond to some sort of jet or rocket propulseion.)115 (defn vector-motor-control116 "Create a function that accepts a sequence of Vector3f objects that117 describe the torque to be applied to each part of the body."118 [body]119 (let [nodes (node-seq body)120 controls (keep #(.getControl % RigidBodyControl) nodes)]121 (fn [torques]122 (map #(.applyTorque %1 %2)123 controls torques))))124 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;125 #+end_src127 ## note -- might want to add a lower dimensional, discrete version of128 ## this if it proves useful from a x-modal clustering perspective.130 * Examples132 #+name: test-body133 #+begin_src clojure134 (ns cortex.test.body135 (:use (cortex world util body))136 (:import137 com.jme3.math.Vector3f138 com.jme3.math.ColorRGBA139 com.jme3.bullet.joints.Point2PointJoint140 com.jme3.bullet.control.RigidBodyControl141 com.jme3.system.NanoTimer))143 (defn worm-segments144 "Create multiple evenly spaced box segments. They're fabulous!"145 [segment-length num-segments interstitial-space radius]146 (letfn [(nth-segment147 [n]148 (box segment-length radius radius :mass 0.1149 :position150 (Vector3f.151 (* 2 n (+ interstitial-space segment-length)) 0 0)152 :name (str "worm-segment" n)153 :color (ColorRGBA/randomColor)))]154 (map nth-segment (range num-segments))))156 (defn connect-at-midpoint157 "Connect two physics objects with a Point2Point joint constraint at158 the point equidistant from both objects' centers."159 [segmentA segmentB]160 (let [centerA (.getWorldTranslation segmentA)161 centerB (.getWorldTranslation segmentB)162 midpoint (.mult (.add centerA centerB) (float 0.5))163 pivotA (.subtract midpoint centerA)164 pivotB (.subtract midpoint centerB)166 ;; A side-effect of creating a joint registers167 ;; it with both physics objects which in turn168 ;; will register the joint with the physics system169 ;; when the simulation is started.170 joint (Point2PointJoint.171 (.getControl segmentA RigidBodyControl)172 (.getControl segmentB RigidBodyControl)173 pivotA174 pivotB)]175 segmentB))177 (defn eve-worm178 "Create a worm-like body bound by invisible joint constraints."179 []180 (let [segments (worm-segments 0.2 5 0.1 0.1)]181 (dorun (map (partial apply connect-at-midpoint)182 (partition 2 1 segments)))183 (nodify "worm" segments)))185 (defn worm-pattern186 "This is a simple, mindless motor control pattern that drives the187 second segment of the worm's body at an offset angle with188 sinusoidally varying strength."189 [time]190 (let [angle (* Math/PI (/ 9 20))191 direction (Vector3f. 0 (Math/sin angle) (Math/cos angle))]192 [Vector3f/ZERO193 (.mult194 direction195 (float (* 2 (Math/sin (* Math/PI 2 (/ (rem time 300 ) 300))))))196 Vector3f/ZERO197 Vector3f/ZERO198 Vector3f/ZERO]))200 (defn test-motor-control201 "Testing motor-control:202 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 "Testing proprioception:230 You should see two foating bars, and a printout of pitch, yaw, and231 roll. Pressing key-r/key-t should move the blue bar up and down and232 change only the value of pitch. key-f/key-g moves it side to side233 and changes yaw. key-v/key-b will spin the blue segment clockwise234 and counterclockwise, and only affect roll."235 []236 (let [hand (box 1 0.2 0.2 :position (Vector3f. 0 2 0)237 :mass 0 :color ColorRGBA/Green)238 finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0)239 :mass 1 :color (ColorRGBA. 0.20 0.40 0.99 1.0))240 floor (box 10 0.5 10 :position (Vector3f. 0 -5 0)241 :mass 0 :color ColorRGBA/Gray)243 move-up? (atom false)244 move-down? (atom false)245 move-left? (atom false)246 move-right? (atom false)247 roll-left? (atom false)248 roll-right? (atom false)249 control (.getControl finger RigidBodyControl)250 joint251 (doto252 (Point2PointJoint.253 (.getControl hand RigidBodyControl)254 control255 (Vector3f. 1.2 0 0)256 (Vector3f. -1.2 0 0 ))257 (.setCollisionBetweenLinkedBodys false))258 time (atom 0)]259 (world260 (nodify [hand finger floor])261 (merge standard-debug-controls262 {"key-r" (fn [_ pressed?] (reset! move-up? pressed?))263 "key-t" (fn [_ pressed?] (reset! move-down? pressed?))264 "key-f" (fn [_ pressed?] (reset! move-left? pressed?))265 "key-g" (fn [_ pressed?] (reset! move-right? pressed?))266 "key-v" (fn [_ pressed?] (reset! roll-left? pressed?))267 "key-b" (fn [_ pressed?] (reset! roll-right? pressed?))})268 (fn [world]269 (.setTimer world (NanoTimer.))270 (set-gravity world (Vector3f. 0 0 0))271 (.setMoveSpeed (.getFlyByCamera world) 50)272 (.setRotationSpeed (.getFlyByCamera world) 50)273 (light-up-everything world))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 (do302 (apply303 (comp304 println-repl305 #(format "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n" %1 %2 %3))306 (joint-proprioception joint))))))))307 #+end_src310 * COMMENT code-limbo311 #+begin_src clojure312 ;;(.loadModel313 ;; (doto (asset-manager)314 ;; (.registerLoader BlenderModelLoader (into-array String ["blend"])))315 ;; "Models/person/person.blend")318 (defn load-blender-model319 "Load a .blend file using an asset folder relative path."320 [^String model]321 (.loadModel322 (doto (asset-manager)323 (.registerLoader BlenderModelLoader (into-array String ["blend"])))324 model))327 (defn view-model [^String model]328 (view329 (.loadModel330 (doto (asset-manager)331 (.registerLoader BlenderModelLoader (into-array String ["blend"])))332 model)))334 (defn load-blender-scene [^String model]335 (.loadModel336 (doto (asset-manager)337 (.registerLoader BlenderLoader (into-array String ["blend"])))338 model))340 (defn worm341 []342 (.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml"))344 (defn oto345 []346 (.loadModel (asset-manager) "Models/Oto/Oto.mesh.xml"))348 (defn sinbad349 []350 (.loadModel (asset-manager) "Models/Sinbad/Sinbad.mesh.xml"))352 (defn worm-blender353 []354 (first (seq (.getChildren (load-blender-model355 "Models/anim2/simple-worm.blend")))))357 (defn body358 "given a node with a SkeletonControl, will produce a body sutiable359 for AI control with movement and proprioception."360 [node]361 (let [skeleton-control (.getControl node SkeletonControl)362 krc (KinematicRagdollControl.)]363 (comment364 (dorun365 (map #(.addBoneName krc %)366 ["mid2" "tail" "head" "mid1" "mid3" "mid4" "Dummy-Root" ""]367 ;;"mid2" "mid3" "tail" "head"]368 )))369 (.addControl node krc)370 (.setRagdollMode krc)371 )372 node373 )374 (defn show-skeleton [node]375 (let [sd377 (doto378 (SkeletonDebugger. "aurellem-skel-debug"379 (skel node))380 (.setMaterial (green-x-ray)))]381 (.attachChild node sd)382 node))386 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;388 ;; this could be a good way to give objects special properties like389 ;; being eyes and the like391 (.getUserData392 (.getChild393 (load-blender-model "Models/property/test.blend") 0)394 "properties")396 ;; the properties are saved along with the blender file.397 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;402 (defn init-debug-skel-node403 [f debug-node skeleton]404 (let [bones405 (map #(.getBone skeleton %)406 (range (.getBoneCount skeleton)))]407 (dorun (map #(.setUserControl % true) bones))408 (dorun (map (fn [b]409 (println (.getName b)410 " -- " (f b)))411 bones))412 (dorun413 (map #(.attachChild414 debug-node415 (doto416 (sphere 0.1417 :position (f %)418 :physical? false)419 (.setMaterial (green-x-ray))))420 bones)))421 debug-node)423 (import jme3test.bullet.PhysicsTestHelper)426 (defn test-zzz [the-worm world value]427 (if (not value)428 (let [skeleton (skel the-worm)]429 (println-repl "enabling bones")430 (dorun431 (map432 #(.setUserControl (.getBone skeleton %) true)433 (range (.getBoneCount skeleton))))436 (let [b (.getBone skeleton 2)]437 (println-repl "moving " (.getName b))438 (println-repl (.getLocalPosition b))439 (.setUserTransforms b440 Vector3f/UNIT_X441 Quaternion/IDENTITY442 ;;(doto (Quaternion.)443 ;; (.fromAngles (/ Math/PI 2)444 ;; 0445 ;; 0447 (Vector3f. 1 1 1))448 )450 (println-repl "hi! <3"))))453 (defn test-ragdoll []455 (let [the-worm457 ;;(.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml")458 (doto (show-skeleton (worm-blender))459 (.setLocalTranslation (Vector3f. 0 10 0))460 ;;(worm)461 ;;(oto)462 ;;(sinbad)463 )464 ]467 (.start468 (world469 (doto (Node.)470 (.attachChild the-worm))471 {"key-return" (fire-cannon-ball)472 "key-space" (partial test-zzz the-worm)473 }474 (fn [world]475 (light-up-everything world)476 (PhysicsTestHelper/createPhysicsTestWorld477 (.getRootNode world)478 (asset-manager)479 (.getPhysicsSpace480 (.getState (.getStateManager world) BulletAppState)))481 (set-gravity world Vector3f/ZERO)482 ;;(.setTimer world (NanoTimer.))483 ;;(org.lwjgl.input.Mouse/setGrabbed false)484 )485 no-op486 )489 )))492 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;493 ;;; here is the ragdoll stuff495 (def worm-mesh (.getMesh (.getChild (worm-blender) 0)))496 (def mesh worm-mesh)498 (.getFloatBuffer mesh VertexBuffer$Type/Position)499 (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)500 (.getData (.getBuffer mesh VertexBuffer$Type/BoneIndex))503 (defn position [index]504 (.get505 (.getFloatBuffer worm-mesh VertexBuffer$Type/Position)506 index))508 (defn bones [index]509 (.get510 (.getData (.getBuffer mesh VertexBuffer$Type/BoneIndex))511 index))513 (defn bone-weights [index]514 (.get515 (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)516 index))520 (defn vertex-bones [vertex]521 (vec (map (comp int bones) (range (* vertex 4) (+ (* vertex 4) 4)))))523 (defn vertex-weights [vertex]524 (vec (map (comp float bone-weights) (range (* vertex 4) (+ (* vertex 4) 4)))))526 (defn vertex-position [index]527 (let [offset (* index 3)]528 (Vector3f. (position offset)529 (position (inc offset))530 (position (inc(inc offset))))))532 (def vertex-info (juxt vertex-position vertex-bones vertex-weights))534 (defn bone-control-color [index]535 (get {[1 0 0 0] ColorRGBA/Red536 [1 2 0 0] ColorRGBA/Magenta537 [2 0 0 0] ColorRGBA/Blue}538 (vertex-bones index)539 ColorRGBA/White))541 (defn influence-color [index bone-num]542 (get543 {(float 0) ColorRGBA/Blue544 (float 0.5) ColorRGBA/Green545 (float 1) ColorRGBA/Red}546 ;; find the weight of the desired bone547 ((zipmap (vertex-bones index)(vertex-weights index))548 bone-num)549 ColorRGBA/Blue))551 (def worm-vertices (set (map vertex-info (range 60))))554 (defn test-info []555 (let [points (Node.)]556 (dorun557 (map #(.attachChild points %)558 (map #(sphere 0.01559 :position (vertex-position %)560 :color (influence-color % 1)561 :physical? false)562 (range 60))))563 (view points)))566 (defrecord JointControl [joint physics-space]567 PhysicsControl568 (setPhysicsSpace [this space]569 (dosync570 (ref-set (:physics-space this) space))571 (.addJoint space (:joint this)))572 (update [this tpf])573 (setSpatial [this spatial])574 (render [this rm vp])575 (getPhysicsSpace [this] (deref (:physics-space this)))576 (isEnabled [this] true)577 (setEnabled [this state]))579 (defn add-joint580 "Add a joint to a particular object. When the object is added to the581 PhysicsSpace of a simulation, the joint will also be added"582 [object joint]583 (let [control (JointControl. joint (ref nil))]584 (.addControl object control))585 object)588 (defn hinge-world589 []590 (let [sphere1 (sphere)591 sphere2 (sphere 1 :position (Vector3f. 3 3 3))592 joint (Point2PointJoint.593 (.getControl sphere1 RigidBodyControl)594 (.getControl sphere2 RigidBodyControl)595 Vector3f/ZERO (Vector3f. 3 3 3))]596 (add-joint sphere1 joint)597 (doto (Node. "hinge-world")598 (.attachChild sphere1)599 (.attachChild sphere2))))602 (defn test-joint []603 (view (hinge-world)))605 ;; (defn copier-gen []606 ;; (let [count (atom 0)]607 ;; (fn [in]608 ;; (swap! count inc)609 ;; (clojure.contrib.duck-streams/copy610 ;; in (File. (str "/home/r/tmp/mao-test/clojure-images/"611 ;; ;;/home/r/tmp/mao-test/clojure-images612 ;; (format "%08d.png" @count)))))))613 ;; (defn decrease-framerate []614 ;; (map615 ;; (copier-gen)616 ;; (sort617 ;; (map first618 ;; (partition619 ;; 4620 ;; (filter #(re-matches #".*.png$" (.getCanonicalPath %))621 ;; (file-seq622 ;; (file-str623 ;; "/home/r/media/anime/mao-temp/images"))))))))627 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;629 (defn proprioception630 "Create a proprioception map that reports the rotations of the631 various limbs of the creature's body"632 [creature]633 [#^Node creature]634 (let [635 nodes (node-seq creature)636 joints637 (map638 :joint639 (filter640 #(isa? (class %) JointControl)641 (reduce642 concat643 (map (fn [node]644 (map (fn [num] (.getControl node num))645 (range (.getNumControls node))))646 nodes))))]647 (fn []648 (reduce concat (map relative-positions (list (first joints)))))))651 (defn skel [node]652 (doto653 (.getSkeleton654 (.getControl node SkeletonControl))655 ;; this is necessary to force the skeleton to have accurate world656 ;; transforms before it is rendered to the screen.657 (.resetAndUpdate)))659 (defn green-x-ray []660 (doto (Material. (asset-manager)661 "Common/MatDefs/Misc/Unshaded.j3md")662 (.setColor "Color" ColorRGBA/Green)663 (-> (.getAdditionalRenderState)664 (.setDepthTest false))))666 (defn test-worm []667 (.start668 (world669 (doto (Node.)670 ;;(.attachChild (point-worm))671 (.attachChild (load-blender-model672 "Models/anim2/joint-worm.blend"))674 (.attachChild (box 10 1 10675 :position (Vector3f. 0 -2 0) :mass 0676 :color (ColorRGBA/Gray))))677 {678 "key-space" (fire-cannon-ball)679 }680 (fn [world]681 (enable-debug world)682 (light-up-everything world)683 ;;(.setTimer world (NanoTimer.))684 )685 no-op)))689 ;; defunct movement stuff690 (defn torque-controls [control]691 (let [torques692 (concat693 (map #(Vector3f. 0 (Math/sin %) (Math/cos %))694 (range 0 (* Math/PI 2) (/ (* Math/PI 2) 20)))695 [Vector3f/UNIT_X])]696 (map (fn [torque-axis]697 (fn [torque]698 (.applyTorque699 control700 (.mult (.mult (.getPhysicsRotation control)701 torque-axis)702 (float703 (* (.getMass control) torque))))))704 torques)))706 (defn motor-map707 "Take a creature and generate a function that will enable fine708 grained control over all the creature's limbs."709 [#^Node creature]710 (let [controls (keep #(.getControl % RigidBodyControl)711 (node-seq creature))712 limb-controls (reduce concat (map torque-controls controls))713 body-control (partial map #(%1 %2) limb-controls)]714 body-control))716 (defn test-motor-map717 "see how torque works."718 []719 (let [finger (box 3 0.5 0.5 :position (Vector3f. 0 2 0)720 :mass 1 :color ColorRGBA/Green)721 motor-map (motor-map finger)]722 (world723 (nodify [finger724 (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0725 :color ColorRGBA/Gray)])726 standard-debug-controls727 (fn [world]728 (set-gravity world Vector3f/ZERO)729 (light-up-everything world)730 (.setTimer world (NanoTimer.)))731 (fn [_ _]732 (dorun (motor-map [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0]))))))733 #+end_src741 * COMMENT generate Source742 #+begin_src clojure :tangle ../src/cortex/body.clj743 <<proprioception>>744 <<motor-control>>745 #+end_src747 #+begin_src clojure :tangle ../src/cortex/test/body.clj748 <<test-body>>749 #+end_src