# HG changeset patch # User Robert McIntyre # Date 1328100231 25200 # Node ID 421cc43441ae7b90c9899dc5ccf67a16a3403ad3 # Parent ac350a0ac6b0f7ad77fafb7cf5278312f749d22f cleaned up test, moved some functions diff -r ac350a0ac6b0 -r 421cc43441ae org/body.org --- a/org/body.org Wed Feb 01 02:44:07 2012 -0700 +++ b/org/body.org Wed Feb 01 05:43:51 2012 -0700 @@ -15,23 +15,67 @@ com.jme3.math.Quaternion com.jme3.math.Vector2f com.jme3.math.Matrix3f - com.jme3.bullet.control.RigidBodyControl)) + com.jme3.bullet.control.RigidBodyControl + com.jme3.collision.CollisionResults + com.jme3.bounding.BoundingBox)) (import com.jme3.scene.Node) +(defn jme-to-blender + "Convert from JME coordinates to Blender coordinates" + [#^Vector3f in] + (Vector3f. (.getX in) + (- (.getZ in)) + (.getY in))) + +(defn joint-targets + "Return the two closest two objects to the joint object, ordered + from bottom to top according to the joint's rotation." + [#^Node parts #^Node joint] + (loop [radius (float 0.01)] + (let [results (CollisionResults.)] + (.collideWith + parts + (BoundingBox. (.getWorldTranslation joint) + radius radius radius) + results) + (let [targets + (distinct + (map #(.getGeometry %) results))] + (if (>= (count targets) 2) + (sort-by + #(let [v + (jme-to-blender + (.mult + (.inverse (.getWorldRotation joint)) + (.subtract (.getWorldTranslation %) + (.getWorldTranslation joint))))] + (println-repl (.getName %) ":" v) + (.dot (Vector3f. 1 1 1) + v)) + (take 2 targets)) + (recur (float (* radius 2)))))))) + +(defn creature-joints + "Return the children of the creature's \"joints\" node." + [#^Node creature] + (if-let [joint-node (.getChild creature "joints")] + (seq (.getChildren joint-node)) + (do (println-repl "could not find JOINTS node") []))) + (defn joint-proprioception [#^Node parts #^Node joint] - (let [[obj-a obj-b] (cortex.silly/joint-targets parts joint) + (let [[obj-a obj-b] (joint-targets parts joint) joint-rot (.getWorldRotation joint) x (.mult joint-rot Vector3f/UNIT_X) y (.mult joint-rot Vector3f/UNIT_Y) z (.mult joint-rot Vector3f/UNIT_Z)] ;; this function will report proprioceptive information for the - ;; joint + ;; joint. (fn [] ;; x is the "twist" axis, y and z are the "bend" axes (let [rot-a (.getWorldRotation obj-a) rot-b (.getWorldRotation obj-b) - relative (.mult (.inverse rot-a) rot-b) + relative (.mult rot-b (.inverse rot-a)) basis (doto (Matrix3f.) (.setColumn 0 x) (.setColumn 1 y) @@ -44,7 +88,6 @@ [yaw roll pitch] (seq (.toAngles rotation-about-joint nil))] ;;return euler angles of the quaternion around the new basis - ;;[yaw pitch roll] [yaw roll pitch] )))) @@ -54,7 +97,7 @@ entire body." [#^Node creature] ;; extract the body's joints - (let [joints (cortex.silly/creature-joints creature) + (let [joints (creature-joints creature) senses (map (partial joint-proprioception creature) joints)] (fn [] (map #(%) senses)))) @@ -95,6 +138,7 @@ #+begin_src clojure (ns cortex.test.body (:use (cortex world util body)) + (:require cortex.silly) (:import com.jme3.math.Vector3f com.jme3.math.ColorRGBA @@ -188,7 +232,7 @@ (worm-pattern @time))))))) -(require 'cortex.silly) + (defn join-at-point [obj-a obj-b world-pivot] (cortex.silly/joint-dispatch {:type :point} @@ -287,6 +331,13 @@ +(defn tap [obj direction force] + (let [control (.getControl obj RigidBodyControl)] + (.applyTorque + control + (.mult (.getPhysicsRotation control) + (.mult (.normalize direction) (float force)))))) + (defn test-proprioception @@ -301,11 +352,25 @@ :mass 0 :color ColorRGBA/Green :name "hand") finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0) :mass 1 :color ColorRGBA/Red :name "finger") - floor (box 10 10 10 :position (Vector3f. 0 -15 0) - :mass 0 :color ColorRGBA/Gray) joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow :position (Vector3f. 1.2 2 0) :physical? false) + joint (join-at-point hand finger (Vector3f. 1.2 2 0 )) + creature (nodify [hand finger joint-node]) + ;; ******************************************* + hand2 (box 1 0.2 0.2 :position (Vector3f. 0 1.5 -3) + :mass 0 :color ColorRGBA/Blue) + finger2 (box 1 0.2 0.2 :position (Vector3f. 2.4 1.5 -3) + :mass 1 :color ColorRGBA/Magenta) + joint-node2 (box 0.1 0.05 0.05 :color ColorRGBA/Gray + :position (Vector3f. 1.2 1.5 -3) + :physical? false) + joint2 (join-at-point hand2 finger2 (Vector3f. 1.2 1.5 -3)) + creature2 (nodify [hand2 finger2 joint-node2]) + ;; ******************************************* + + floor (box 10 10 10 :position (Vector3f. 0 -15 0) + :mass 0 :color ColorRGBA/Gray) move-up? (atom false) move-down? (atom false) @@ -313,27 +378,23 @@ move-right? (atom false) roll-left? (atom false) roll-right? (atom false) - control (.getControl finger RigidBodyControl) - time (atom 0) - joint (join-at-point hand finger (Vector3f. 1.2 2 0 )) - creature (nodify [hand finger joint-node]) + + + root (nodify [creature creature2 floor]) prop (joint-proprioception creature joint-node) - - prop-view (proprioception-debug-window) - - - ] + prop-view (proprioception-debug-window)] - - (.setCollisionGroup - (.getControl hand RigidBodyControl) - PhysicsCollisionObject/COLLISION_GROUP_NONE) + (comment + (.setCollisionGroup + (.getControl hand RigidBodyControl) + PhysicsCollisionObject/COLLISION_GROUP_NONE) + ) (world - (nodify [hand finger floor joint-node]) + root (merge standard-debug-controls {"key-r" (fn [_ pressed?] (reset! move-up? pressed?)) "key-t" (fn [_ pressed?] (reset! move-down? pressed?)) @@ -346,33 +407,20 @@ (set-gravity world (Vector3f. 0 0 0)) (light-up-everything world)) (fn [_ _] - (if @move-up? - (.applyTorque control - (.mult (.getPhysicsRotation control) - (Vector3f. 0 0 10)))) - (if @move-down? - (.applyTorque control - (.mult (.getPhysicsRotation control) - (Vector3f. 0 0 -10)))) - (if @move-left? - (.applyTorque control - (.mult (.getPhysicsRotation control) - (Vector3f. 0 10 0)))) - (if @move-right? - (.applyTorque control - (.mult (.getPhysicsRotation control) - (Vector3f. 0 -10 0)))) - (if @roll-left? - (.applyTorque control - (.mult (.getPhysicsRotation control) - (Vector3f. -1 0 0)))) - (if @roll-right? - (.applyTorque control - (.mult (.getPhysicsRotation control) - (Vector3f. 1 0 0)))) - - ;;(if (= 0 (rem (swap! time inc) 20)) - (prop-view (list (prop))))))) + (let [force 10 + left (Vector3f. 0 1 0) + right (Vector3f. 0 -1 0) + up (Vector3f. 0 0 1) + down (Vector3f. 0 0 -1) + roll-left (Vector3f. -1 0 0) + roll-right (Vector3f. 1 0 0)] + (if @move-up? (tap finger up force)) + (if @move-down? (tap finger down force)) + (if @move-left? (tap finger left force)) + (if @move-right? (tap finger right force)) + (if @roll-left? (tap finger roll-left (/ force 10))) + (if @roll-right? (tap finger roll-right (/ force 10)))) + (prop-view (list (prop))))))) #+end_src diff -r ac350a0ac6b0 -r 421cc43441ae org/test-creature.org --- a/org/test-creature.org Wed Feb 01 02:44:07 2012 -0700 +++ b/org/test-creature.org Wed Feb 01 05:43:51 2012 -0700 @@ -262,73 +262,7 @@ (.getZ in) (- (.getY in)))) -(defn jme-to-blender - "Convert from JME coordinates to Blender coordinates" - [#^Vector3f in] - (Vector3f. (.getX in) - (- (.getZ in)) - (.getY in))) -(defn joint-targets - "Return the two closest two objects to the joint object, ordered - from bottom to top according to the joint's rotation." - [#^Node parts #^Node joint] - (loop [radius (float 0.01)] - (let [results (CollisionResults.)] - (.collideWith - parts - (BoundingBox. (.getWorldTranslation joint) - radius radius radius) - results) - (let [targets - (distinct - (map #(.getGeometry %) results))] - (if (>= (count targets) 2) - (sort-by - #(let [v - (jme-to-blender - (.mult - (.inverse (.getWorldRotation joint)) - (.subtract (.getWorldTranslation %) - (.getWorldTranslation joint))))] - (println-repl (.getName %) ":" v) - (.dot (Vector3f. 1 1 1) - v)) - (take 2 targets)) - (recur (float (* radius 2)))))))) - - -(defn proprio-joint [#^Node parts #^Node joint] - (let [[obj-a obj-b] (joint-targets parts joint) - joint-rot (.getWorldRotation joint) - x (.mult joint-rot Vector3f/UNIT_X) - y (.mult joint-rot Vector3f/UNIT_Y) - z (.mult joint-rot Vector3f/UNIT_Z)] - ;; this function will report proprioceptive information for the - ;; joint - (fn [] - ;; x is the "twist" axis, y and z are the "bend" axes - (let [rot-a (.getWorldRotation obj-a) - rot-b (.getWorldRotation obj-b) - relative (.mult (.inverse rot-a) rot-b) - basis (doto (Matrix3f.) - (.setColumn 0 y) - (.setColumn 1 z) - (.setColumn 2 x)) - rotation-about-joint - (doto (Quaternion.) - (.fromRotationMatrix - (.mult (.inverse basis) - (.toRotationMatrix relative)))) - - confirm-axes - (let [temp-axes (make-array Vector3f 3)] - (.toAxes rotation-about-joint temp-axes) - (seq temp-axes)) - euler-angles - (seq (.toAngles rotation-about-joint nil))] - ;;return euler angles of the quaternion around the new basis - euler-angles)))) @@ -1058,12 +992,6 @@ ;; these are the functions that provide world i/o, chinese-room style -(defn creature-joints - "Return the children of the creature's \"joints\" node." - [#^Node creature] - (if-let [joint-node (.getChild creature "joints")] - (seq (.getChildren joint-node)) - (do (println-repl "could not find JOINTS node") []))) (defn blender-creature @@ -1156,24 +1084,6 @@ (.getRGB image x 0))))) -(defn rad->deg [rad] - (* 180 (/ Math/PI) rad)) - - -(defn debug-prop-window - "create a debug view for proprioception" - [] - (let [vi (view-image)] - (fn [sensor-data] - (println-repl - (map - (fn [[yaw pitch roll]] - [(rad->deg yaw) - (rad->deg pitch) - (rad->deg roll)]) - sensor-data))))) - - (defn draw-sprite [image sprite x y color ] (dorun (for [[u v] sprite] @@ -1239,7 +1149,7 @@ bell (AudioNode. (asset-manager) "Sounds/pure.wav" false) prop (proprioception creature) - prop-debug (debug-prop-window) + prop-debug (proprioception-debug-window) ;; dream ]