# HG changeset patch # User Robert McIntyre # Date 1328212151 25200 # Node ID 7a49b81ca1bf9646992c5e815a36704ad2dbb580 # Parent 48f9cba082ebc883b4c302faf4bcabd38eaf53ea finally got proprioception working to my satisfaction diff -r 48f9cba082eb -r 7a49b81ca1bf assets/Models/creature1/try-again.blend Binary file assets/Models/creature1/try-again.blend has changed diff -r 48f9cba082eb -r 7a49b81ca1bf org/body.org --- a/org/body.org Thu Feb 02 03:17:11 2012 -0700 +++ b/org/body.org Thu Feb 02 12:49:11 2012 -0700 @@ -63,42 +63,50 @@ (seq (.getChildren joint-node)) (do (println-repl "could not find JOINTS node") []))) +(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 joint-proprioception [#^Node parts #^Node joint] (let [[obj-a obj-b] (joint-targets parts joint) joint-rot (.getWorldRotation joint) - pre-inv-a (.inverse (.getWorldRotation obj-a)) - x (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_X)) - y (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Y)) - z (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Z)) - tmp-rot-a (.getWorldRotation obj-a)] - (println-repl "x:" (.mult tmp-rot-a x)) - (println-repl "y:" (.mult tmp-rot-a y)) - (println-repl "z:" (.mult tmp-rot-a z)) - (println-repl "rot-a" (.getWorldRotation obj-a)) - (println-repl "rot-b" (.getWorldRotation obj-b)) - ;; this function will report proprioceptive information for the - ;; joint. + x0 (.mult joint-rot Vector3f/UNIT_X) + y0 (.mult joint-rot Vector3f/UNIT_Y) + z0 (.mult joint-rot Vector3f/UNIT_Z)] + (println-repl "x:" x0) + (println-repl "y:" y0) + (println-repl "z:" z0) + (println-repl "init-a:" (.getWorldRotation obj-a)) + (println-repl "init-b:" (.getWorldRotation obj-b)) + (fn [] - ;; x is the "twist" axis, y and z are the "bend" axes - (let [rot-a (.getWorldRotation obj-a) - ;;inv-a (.inverse rot-a) - rot-b (.getWorldRotation obj-b) - ;;relative (.mult rot-b inv-a) - basis (doto (Matrix3f.) - (.setColumn 0 (.mult rot-a x)) - (.setColumn 1 (.mult rot-a y)) - (.setColumn 2 (.mult rot-a z))) - rotation-about-joint + (let [rot-a (.clone (.getWorldRotation obj-a)) + rot-b (.clone (.getWorldRotation obj-b)) + x (.mult rot-a x0) + y (.mult rot-a y0) + z (.mult rot-a z0) + + X (.mult rot-b x0) + Y (.mult rot-b y0) + Z (.mult rot-b z0) + heading (Math/atan2 (.dot X z) (.dot X x)) + pitch (Math/atan2 (.dot X y) (.dot X x)) + + ;; rotate x-vector back to origin + reverse (doto (Quaternion.) - (.fromRotationMatrix - (.mult (.invert basis) - (.toRotationMatrix rot-b)))) - [yaw roll pitch] - (seq (.toAngles rotation-about-joint nil))] - ;;return euler angles of the quaternion around the new basis - [yaw roll pitch] - )))) - + (.fromAngleAxis + (.angleBetween X x) + (let [cross (.normalize (.cross X x))] + (if (= 0 (.length cross)) y cross)))) + roll (absolute-angle (.mult reverse Y) y x)] + + [heading pitch roll])))) (defn proprioception "Create a function that provides proprioceptive information about an @@ -161,17 +169,58 @@ world-loop* (fn [world tpf] (world-loop world tpf) (splice-loop))] + [root-node + keymap* + intilization + world-loop*])) - [root-node - keymap* - intilization - world-loop*])) +(import java.awt.image.BufferedImage) - +(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 [heading (view-angle 0xFF0000) + pitch (view-angle 0x00FF00) + roll (view-angle 0xFFFFFF) + v-heading (view-image) + v-pitch (view-image) + v-roll (view-image) + ] + (fn [prop-data] + (dorun + (map + (fn [[h p r]] + (v-heading (heading h)) + (v-pitch (pitch p)) + (v-roll (roll r))) + prop-data))))) + + #+end_src #+results: proprioception -: #'cortex.body/proprioception +: #'cortex.body/proprioception-debug-window * Motor Control #+name: motor-control @@ -210,7 +259,8 @@ com.jme3.math.ColorRGBA com.jme3.bullet.joints.Point2PointJoint com.jme3.bullet.control.RigidBodyControl - com.jme3.system.NanoTimer)) + com.jme3.system.NanoTimer + com.jme3.math.Quaternion)) (defn worm-segments "Create multiple evenly spaced box segments. They're fabulous!" @@ -333,47 +383,6 @@ (set-gravity world Vector3f/ZERO) ) no-op))) -(import java.awt.image.BufferedImage) - -(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))))) (comment (defn proprioception-debug-window @@ -406,25 +415,32 @@ and changes yaw. key-v/key-b will spin the blue segment clockwise and counterclockwise, and only affect roll." [] - (let [hand (box 1 0.2 0.2 :position (Vector3f. 0 2 0) + (let [hand (box 0.2 1 0.2 :position (Vector3f. 0 0 0) :mass 0 :color ColorRGBA/Green :name "hand") - finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0) + finger (box 0.2 1 0.2 :position (Vector3f. 0 2.4 0) :mass 1 :color ColorRGBA/Red :name "finger") joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow - :position (Vector3f. 1.2 2 0) + :position (Vector3f. 0 1.2 0) + :rotation (doto (Quaternion.) + (.fromAngleAxis + (/ Math/PI 2) + (Vector3f. 0 0 1))) :physical? false) - joint (join-at-point hand finger (Vector3f. 1.2 2 0 )) + joint (join-at-point hand finger (Vector3f. 0 1.2 0 )) creature (nodify [hand finger joint-node]) + finger-control (.getControl finger RigidBodyControl) + hand-control (.getControl hand RigidBodyControl)] + + + (let ;; ******************************************* - floor (box 10 10 10 :position (Vector3f. 0 -15 0) + [floor (box 10 10 10 :position (Vector3f. 0 -15 0) :mass 0 :color ColorRGBA/Gray) root (nodify [creature floor]) prop (joint-proprioception creature joint-node) prop-view (proprioception-debug-window) - finger-control (.getControl finger RigidBodyControl) - hand-control (.getControl hand RigidBodyControl) controls (merge standard-debug-controls @@ -471,14 +487,14 @@ (with-movement finger ["key-r" "key-t" "key-f" "key-g" "key-v" "key-b"] - [10 10 10 10 1 1] + [1 1 10 10 10 10] [root controls (fn [world] (.setTimer world (com.aurellem.capture.RatchetTimer. 60)) (set-gravity world (Vector3f. 0 0 0)) (light-up-everything world)) - (fn [_ _] (prop-view (list (prop))))]))))) + (fn [_ _] (prop-view (list (prop))))])))))) #+end_src @@ -908,7 +924,51 @@ (light-up-everything world) (.setTimer world (NanoTimer.))) (fn [_ _] - (dorun (motor-map [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0])))))) + (dorun (motor-map [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 + 0])))))) + +(defn joint-proprioception [#^Node parts #^Node joint] + (let [[obj-a obj-b] (joint-targets parts joint) + joint-rot (.getWorldRotation joint) + pre-inv-a (.inverse (.getWorldRotation obj-a)) + x (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_X)) + y (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Y)) + z (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Z)) + + x Vector3f/UNIT_Y + y Vector3f/UNIT_Z + z Vector3f/UNIT_X + + + tmp-rot-a (.getWorldRotation obj-a)] + (println-repl "x:" (.mult tmp-rot-a x)) + (println-repl "y:" (.mult tmp-rot-a y)) + (println-repl "z:" (.mult tmp-rot-a z)) + (println-repl "rot-a" (.getWorldRotation obj-a)) + (println-repl "rot-b" (.getWorldRotation obj-b)) + (println-repl "joint-rot" joint-rot) + ;; 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) + ;;inv-a (.inverse rot-a) + rot-b (.getWorldRotation obj-b) + ;;relative (.mult rot-b inv-a) + basis (doto (Matrix3f.) + (.setColumn 0 (.mult rot-a x)) + (.setColumn 1 (.mult rot-a y)) + (.setColumn 2 (.mult rot-a z))) + rotation-about-joint + (doto (Quaternion.) + (.fromRotationMatrix + (.mult (.invert basis) + (.toRotationMatrix rot-b)))) + [yaw roll pitch] + (seq (.toAngles rotation-about-joint nil))] + ;;return euler angles of the quaternion around the new basis + [yaw roll pitch])))) + #+end_src diff -r 48f9cba082eb -r 7a49b81ca1bf org/test-creature.org --- a/org/test-creature.org Thu Feb 02 03:17:11 2012 -0700 +++ b/org/test-creature.org Thu Feb 02 12:49:11 2012 -0700 @@ -1086,53 +1086,6 @@ 0xFF (.getRGB image x 0))))) - -(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))))) - - - - - - - (defn test-creature [thing] (let [x-axis (box 1 0.01 0.01 :physical? false :color ColorRGBA/Red) @@ -1190,7 +1143,7 @@ (comp (view-image) BufferedImage!)) (add-eye world (.getCamera world) no-op) - (set-gravity world (Vector3f. 0 0 0)) + ;;(set-gravity world (Vector3f. 0 0 0)) ;;(com.aurellem.capture.Capture/captureVideo ;; world (file-str "/home/r/proj/ai-videos/hand")) ;;(.setTimer world (RatchetTimer. 60))