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

 

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

 

 

 (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

 

 * COMMENT generate source

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

 <<proprioception>>

 #+end_src