1.1 --- a/org/body.org	Fri Feb 03 06:15:34 2012 -0700
     1.2 +++ b/org/body.org	Fri Feb 03 06:21:39 2012 -0700
     1.3 @@ -62,61 +62,6 @@
     1.4      (seq (.getChildren joint-node))
     1.5      (do (println-repl "could not find JOINTS node") [])))
     1.6  
     1.7 -(defn right-handed? [vec1 vec2 vec3]
     1.8 -  (< 0 (.dot (.cross vec1 vec2) vec3)))
     1.9 -
    1.10 -(defn absolute-angle [vec1 vec2 axis]
    1.11 -  (let [angle (.angleBetween vec1 vec2)]
    1.12 -    (if (right-handed? vec1 vec2 axis)
    1.13 -      angle (- (* 2 Math/PI) angle))))
    1.14 -
    1.15 -
    1.16 -(defn joint-proprioception [#^Node parts #^Node joint]
    1.17 -  (let [[obj-a obj-b] (joint-targets parts joint)
    1.18 -        joint-rot (.getWorldRotation joint)
    1.19 -        x0 (.mult joint-rot Vector3f/UNIT_X)
    1.20 -        y0 (.mult joint-rot Vector3f/UNIT_Y)
    1.21 -        z0 (.mult joint-rot Vector3f/UNIT_Z)]
    1.22 -    (println-repl "x:" x0)
    1.23 -    (println-repl "y:" y0)
    1.24 -    (println-repl "z:" z0)
    1.25 -    (println-repl "init-a:" (.getWorldRotation obj-a))
    1.26 -    (println-repl "init-b:" (.getWorldRotation obj-b))
    1.27 -
    1.28 -    (fn []
    1.29 -      (let [rot-a (.clone (.getWorldRotation obj-a))
    1.30 -            rot-b (.clone (.getWorldRotation obj-b))
    1.31 -            x (.mult rot-a x0)
    1.32 -            y (.mult rot-a y0)
    1.33 -            z (.mult rot-a z0)
    1.34 -
    1.35 -            X (.mult rot-b x0)
    1.36 -            Y (.mult rot-b y0)
    1.37 -            Z (.mult rot-b z0)
    1.38 -            heading  (Math/atan2 (.dot X z) (.dot X x))
    1.39 -            pitch  (Math/atan2 (.dot X y) (.dot X x))
    1.40 -
    1.41 -            ;; rotate x-vector back to origin
    1.42 -            reverse
    1.43 -            (doto (Quaternion.)
    1.44 -              (.fromAngleAxis
    1.45 -               (.angleBetween X x)
    1.46 -               (let [cross (.normalize (.cross X x))]
    1.47 -                 (if (= 0 (.length cross)) y cross))))
    1.48 -            roll (absolute-angle (.mult reverse Y) y x)]
    1.49 -        
    1.50 -        [heading pitch roll]))))
    1.51 -
    1.52 -(defn proprioception
    1.53 -  "Create a function that provides proprioceptive information about an
    1.54 -  entire body."
    1.55 -  [#^Node creature]
    1.56 -  ;; extract the body's joints
    1.57 -  (let [joints (creature-joints creature)
    1.58 -        senses (map (partial joint-proprioception creature) joints)]
    1.59 -    (fn []
    1.60 -      (map #(%) senses))))
    1.61 -
    1.62  (defn tap [obj direction force]
    1.63    (let [control (.getControl obj RigidBodyControl)]
    1.64      (.applyTorque

     2.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     2.2 +++ b/org/proprioception.org	Fri Feb 03 06:21:39 2012 -0700
     2.3 @@ -0,0 +1,87 @@
     2.4 +#+title: The Sense of Proprioception
     2.5 +#+author: Robert McIntyre
     2.6 +#+email: rlm@mit.edu
     2.7 +#+description: proprioception for simulated creatures
     2.8 +#+keywords: simulation, jMonkeyEngine3, clojure
     2.9 +#+SETUPFILE: ../../aurellem/org/setup.org
    2.10 +#+INCLUDE: ../../aurellem/org/level-0.org
    2.11 +
    2.12 +#+name: proprioception
    2.13 +#+begin_src clojure
    2.14 +(ns cortex.proprioception
    2.15 +  (:use (cortex world util sense body)))
    2.16 +
    2.17 +(cortex.import/mega-import-jme3)
    2.18 +
    2.19 +(defn right-handed? [vec1 vec2 vec3]
    2.20 +  (< 0 (.dot (.cross vec1 vec2) vec3)))
    2.21 +
    2.22 +(defn absolute-angle [vec1 vec2 axis]
    2.23 +  (let [angle (.angleBetween vec1 vec2)]
    2.24 +    (if (right-handed? vec1 vec2 axis)
    2.25 +      angle (- (* 2 Math/PI) angle))))
    2.26 +
    2.27 +
    2.28 +(defn joint-proprioception [#^Node parts #^Node joint]
    2.29 +  (let [[obj-a obj-b] (joint-targets parts joint)
    2.30 +        joint-rot (.getWorldRotation joint)
    2.31 +        x0 (.mult joint-rot Vector3f/UNIT_X)
    2.32 +        y0 (.mult joint-rot Vector3f/UNIT_Y)
    2.33 +        z0 (.mult joint-rot Vector3f/UNIT_Z)]
    2.34 +    (println-repl "x:" x0)
    2.35 +    (println-repl "y:" y0)
    2.36 +    (println-repl "z:" z0)
    2.37 +    (println-repl "init-a:" (.getWorldRotation obj-a))
    2.38 +    (println-repl "init-b:" (.getWorldRotation obj-b))
    2.39 +
    2.40 +    (fn []
    2.41 +      (let [rot-a (.clone (.getWorldRotation obj-a))
    2.42 +            rot-b (.clone (.getWorldRotation obj-b))
    2.43 +            x (.mult rot-a x0)
    2.44 +            y (.mult rot-a y0)
    2.45 +            z (.mult rot-a z0)
    2.46 +
    2.47 +            X (.mult rot-b x0)
    2.48 +            Y (.mult rot-b y0)
    2.49 +            Z (.mult rot-b z0)
    2.50 +            heading  (Math/atan2 (.dot X z) (.dot X x))
    2.51 +            pitch  (Math/atan2 (.dot X y) (.dot X x))
    2.52 +
    2.53 +            ;; rotate x-vector back to origin
    2.54 +            reverse
    2.55 +            (doto (Quaternion.)
    2.56 +              (.fromAngleAxis
    2.57 +               (.angleBetween X x)
    2.58 +               (let [cross (.normalize (.cross X x))]
    2.59 +                 (if (= 0 (.length cross)) y cross))))
    2.60 +            roll (absolute-angle (.mult reverse Y) y x)]
    2.61 +        
    2.62 +        [heading pitch roll]))))
    2.63 +
    2.64 +(defn proprioception
    2.65 +  "Create a function that provides proprioceptive information about an
    2.66 +  entire body."
    2.67 +  [#^Node creature]
    2.68 +  ;; extract the body's joints
    2.69 +  (let [joints (creature-joints creature)
    2.70 +        senses (map (partial joint-proprioception creature) joints)]
    2.71 +    (fn []
    2.72 +      (map #(%) senses))))
    2.73 +
    2.74 +
    2.75 +
    2.76 +
    2.77 +
    2.78 +#+end_src
    2.79 +
    2.80 +
    2.81 +
    2.82 +
    2.83 +
    2.84 +
    2.85 +
    2.86 +
    2.87 +* COMMENT generate source
    2.88 +#+begin_src clojure :tangle ../src/cortex/proprioception.clj
    2.89 +<<proprioception>>
    2.90 +#+end_src

     3.1 --- a/org/test-creature.org	Fri Feb 03 06:15:34 2012 -0700
     3.2 +++ b/org/test-creature.org	Fri Feb 03 06:21:39 2012 -0700
     3.3 @@ -49,7 +49,7 @@
     3.4  ;; TODO remove this!
     3.5  (require 'cortex.import)
     3.6  (cortex.import/mega-import-jme3)
     3.7 -(use '(cortex world util body hearing touch vision sense))
     3.8 +(use '(cortex world util body hearing touch vision sense proprioception))
     3.9  
    3.10  (rlm.rlm-commands/help)
    3.11  (import java.awt.image.BufferedImage)