#+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

   "Simulate the sense of proprioception (ability to detect the

   relative positions of body parts with repsect to other body parts)

   in jMonkeyEngine3. Reads specially prepared blender files to

   automatically generate proprioceptive senses."

   (:use (cortex world util sense body))

   (:use clojure.contrib.def)

   (:import com.jme3.scene.Node)

   (:import (com.jme3.math Vector3f Quaternion)))

 

 (defvar 

   ^{:arglists '([creature])}

   joints

   (sense-nodes "joints")

   "Return the children of the creature's \"joints\" node.")

 

 (defn right-handed?

   "true iff the three vectors form a right handed coordinate

   system. The three vectors do not have to be normalized or

   orthogonal."

   [vec1 vec2 vec3]

   (< 0 (.dot (.cross vec1 vec2) vec3)))

 

 (defn absolute-angle

   "The angle between 'vec1 and 'vec2. Positive if the angle to get

   from 'vec1 to 'vec2 is counterclockwise around 'axis, and negative

   otherwise." 

   [vec1 vec2 axis]

   (let [angle (.angleBetween vec1 vec2)]

     (if (right-handed? vec1 vec2 axis)

       angle (- (* 2 Math/PI) angle))))

 

 (defn proprioception-fn

   "Returns a function which returns proprioceptive sensory data when

   called inside a running simulation."

   [#^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!

   "Endow the creature with the sense of proprioception. Returns a

    sequence of functions, one for each child of the \"joints\" node in

    the creature, which each report proprioceptive information about

    that joint."

   [#^Node creature]

   ;; extract the body's joints

   (let [senses (map (partial proprioception-fn creature)

                     (joints creature))]

     (fn []

       (map #(%) senses))))

 #+end_src

 

 * COMMENT generate source

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

 <<proprioception>>

 #+end_src