# HG changeset patch # User Robert McIntyre # Date 1328089447 25200 # Node ID ac350a0ac6b0f7ad77fafb7cf5278312f749d22f # Parent 2ed7e60d3821024c0f79f8c30a12d460802d414c proprioception refrence frame is wrong, trying to fix... diff -r 2ed7e60d3821 -r ac350a0ac6b0 assets/Models/creature1/try-again.blend Binary file assets/Models/creature1/try-again.blend has changed diff -r 2ed7e60d3821 -r ac350a0ac6b0 org/body.org --- a/org/body.org Wed Feb 01 02:27:18 2012 -0700 +++ b/org/body.org Wed Feb 01 02:44:07 2012 -0700 @@ -17,144 +17,6 @@ com.jme3.math.Matrix3f com.jme3.bullet.control.RigidBodyControl)) -(comment - (defn joint-proprioception - "Relative position information for a two-part system connected by a - joint. Gives the pitch, yaw, and roll of the 'B' object relative to - the 'A' object, as determined by the joint." - [joint] - (let [object-a (.getUserObject (.getBodyA joint)) - object-b (.getUserObject (.getBodyB joint)) - arm-a - (.normalize - (.subtract - (.localToWorld object-a (.getPivotA joint) nil) - (.getWorldTranslation object-a))) - - ;; this is probably wrong! - rotate-a - (doto (Matrix3f.) - (.fromStartEndVectors arm-a Vector3f/UNIT_X)) - - arm-b - (.mult - rotate-a - (.normalize - (.subtract - (.localToWorld object-b (.getPivotB joint) nil) - (.getWorldTranslation object-b)))) - pitch - (.angleBetween - (.normalize (Vector2f. (.getX arm-b) (.getY arm-b))) - (Vector2f. 1 0)) - yaw - (.angleBetween - (.normalize (Vector2f. (.getX arm-b) (.getZ arm-b))) - (Vector2f. 1 0)) - - roll - (project-quaternion - (.mult - (.getLocalRotation object-b) - (doto (Quaternion.) - (.fromRotationMatrix rotate-a))) - arm-b)] - ;;(println-repl (.getName object-a) (.getName object-b)) - [pitch yaw roll])) -) - -(defn any-orthogonal - "Generate an arbitray (but stable) orthogonal vector to a given - vector." - [vector] - (let [x (.getX vector) - y (.getY vector) - z (.getZ vector)] - (cond - (not= x (float 0)) (Vector3f. (- z) 0 x) - (not= y (float 0)) (Vector3f. 0 (- z) y) - (not= z (float 0)) (Vector3f. 0 (- z) y) - true Vector3f/ZERO))) - -(comment -(defn project-quaternion - "From http://stackoverflow.com/questions/3684269/ - component-of-a-quaternion-rotation-around-an-axis. - - Determine the amount of rotation a quaternion will - cause about a given axis." - [#^Quaternion q #^Vector3f axis] - (let [basis-1 (any-orthogonal axis) - basis-2 (.cross axis basis-1) - rotated (.mult q basis-1) - alpha (.dot basis-1 (.project rotated basis-1)) - beta (.dot basis-2 (.project rotated basis-2))] - (Math/atan2 beta alpha))) -) - -(defn right-handed? [vec1 vec2 vec3] - (< 0 (.dot (.cross vec1 vec2) vec3))) - -(defn absolute-angle [vec1 vec2 axis] - (let [angle (.angleBetween vec1 vec2)] - (if (right-handed? vec1 vec2 axis) - angle (- (* 2 Math/PI) angle)))) - -(defn angle-min [& angles] - (first - (sort-by - (fn [angle] - (let [in-circle (Math/abs (rem angle (* 2 Math/PI)))] - (min in-circle - (- (* Math/PI 2) in-circle)))) - angles))) - -(defn project-quaternion - "From http://stackoverflow.com/questions/3684269/ - component-of-a-quaternion-rotation-around-an-axis. - - Determine the amount of rotation a quaternion will - cause about a given axis." - [#^Quaternion q #^Vector3f axis] - (let [axis (.normalize axis) - basis-1 (.normalize (any-orthogonal axis)) - basis-2 (.cross axis basis-1) - rotated-1 (.mult q basis-1) - basis-1* (.normalize - (.add (.project rotated-1 basis-1) - (.project rotated-1 basis-2))) - rotated-2 (.mult q basis-2) - basis-2* (.normalize - (.add (.project rotated-2 basis-1) - (.project rotated-2 basis-2))) - angle-1 - (absolute-angle basis-1 basis-1* axis) - angle-2 - (absolute-angle basis-2 basis-2* axis) - - - angle (angle-min angle-1 angle-2) - ] - - - ;; be sure to get sign from cross product - (if false - (do - (println-repl "axis" axis) - (println-repl "basis-1" basis-1) - (println-repl "basis-2" basis-2) - (println-repl "rotated-1" rotated-1) - (println-repl "rotated-2" rotated-2) - (println-repl "basis-1*" basis-1*) - (println-repl "basis-2*" basis-2*) - (println-repl "angle-1" angle-1) - (println-repl "angle-2" angle-2) - - (println-repl "angle" angle) - (println-repl ""))) - angle)) - - (import com.jme3.scene.Node) (defn joint-proprioception [#^Node parts #^Node joint] @@ -187,53 +49,15 @@ )))) -(comment - -(defn joint-proprioception - [joint] - (let [object-a (.getUserObject (.getBodyA joint)) - object-b (.getUserObject (.getBodyB joint)) - rot-a (.clone (.getWorldRotation object-a)) - rot-b (.clone (.getWorldRotation object-b)) - ] - - (.mult rot-b (.inverse rot-a)) - - ;; object-a == hand - ;; object-b == finger - )) -) -;; (defn joint-proprioception* -;; [joint] -;; (let [object-a (.getUserObject (.getBodyA joint)) -;; object-b (.getUserObject (.getBodyB joint)) - -;; rotate-a (.clone (.getWorldRotation object-a)) -;; rotate-b (.clone (.getWorldRotation object-b)) - -;; rotate-rel (.mult rotate-b (.inverse rotate-a)) -;; ] -;; ((comp vec map) (partial project-quaternion rotate-rel) -;; [Vector3f/UNIT_X -;; Vector3f/UNIT_Y -;; Vector3f/UNIT_Z]))) - - (defn proprioception "Create a function that provides proprioceptive information about an entire body." - [body] + [#^Node creature] ;; extract the body's joints - (let [joints - (distinct - (reduce - concat - (map #(.getJoints %) - (keep - #(.getControl % RigidBodyControl) - (node-seq body)))))] + (let [joints (cortex.silly/creature-joints creature) + senses (map (partial joint-proprioception creature) joints)] (fn [] - (map joint-proprioception joints)))) + (map #(%) senses)))) #+end_src diff -r 2ed7e60d3821 -r ac350a0ac6b0 org/test-creature.org --- a/org/test-creature.org Wed Feb 01 02:27:18 2012 -0700 +++ b/org/test-creature.org Wed Feb 01 02:44:07 2012 -0700 @@ -1058,16 +1058,20 @@ ;; 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 "Return a creature with all joints in place." [blender-path] (let [model (load-blender-model blender-path) - joints - (if-let [joint-node (.getChild model "joints")] - (seq (.getChildren joint-node)) - (do (println-repl "could not find joints node") []))] - (assemble-creature model joints))) + joints (creature-joints model)] + (assemble-creature model joints))) (defn gray-scale [num] (+ num @@ -1167,7 +1171,49 @@ [(rad->deg yaw) (rad->deg pitch) (rad->deg roll)]) - sensor-data))))) + sensor-data))))) + + +(defn draw-sprite [image sprite x y color ] + (dorun + (for [[u v] sprite] + (.setRGB image (+ u x) (+ v y) color)))) + +(defn view-angle + "create a debug view of an angle" + [color] + (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB) + previous (atom [25 25]) + sprite [[0 0] [0 1] + [0 -1] [-1 0] [1 0]]] + (fn [angle] + (let [angle (float angle)] + (let [position + [(+ 25 (int (* 20 (Math/cos angle)))) + (+ 25 (int (* 20(Math/sin angle))))]] + (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000) + (draw-sprite image sprite (position 0) (position 1) color) + (reset! previous position)) + image)))) + +(defn proprioception-debug-window + [] + (let [yaw (view-angle 0xFF0000) + roll (view-angle 0x00FF00) + pitch (view-angle 0xFFFFFF) + v-yaw (view-image) + v-roll (view-image) + v-pitch (view-image) + ] + (fn [prop-data] + (dorun + (map + (fn [[y r p]] + (v-yaw (yaw y)) + (v-roll (roll r)) + (v-pitch (pitch p))) + prop-data))))) +