#+title: The Sense of Proprioception

 #+author: Robert McIntyre

 #+email: rlm@mit.edu

 #+description: proprioception for simulated creatures

 #+keywords: simulation, jMonkeyEngine3, clojure

 #+SETUPFILE: ../../aurellem/org/setup.org

 #+INCLUDE: ../../aurellem/org/level-0.org

 

 #+name: proprioception

 #+begin_src clojure

 (ns cortex.proprioception

   (:use (cortex world util sense body)))

 

 (cortex.import/mega-import-jme3)

 

 (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)

         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 []

       (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.)

               (.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

   entire body."

   [#^Node creature]

   ;; extract the body's joints

   (let [joints (creature-joints creature)

         senses (map (partial joint-proprioception creature) joints)]

     (fn []

       (map #(%) senses))))

 

 

 

 

 

 #+end_src

 

 

 

 

 

 

 

 

 * COMMENT generate source

 #+begin_src clojure :tangle ../src/cortex/proprioception.clj

 <<proprioception>>

 #+end_src