rlm@157: #+title: The Sense of Proprioception
rlm@157: #+author: Robert McIntyre
rlm@157: #+email: rlm@mit.edu
rlm@157: #+description: proprioception for simulated creatures
rlm@157: #+keywords: simulation, jMonkeyEngine3, clojure
rlm@157: #+SETUPFILE: ../../aurellem/org/setup.org
rlm@157: #+INCLUDE: ../../aurellem/org/level-0.org
rlm@157: 
rlm@157: #+name: proprioception
rlm@157: #+begin_src clojure
rlm@157: (ns cortex.proprioception
rlm@173:   "Simulate the sense of proprioception (ability to detect the
rlm@173:   relative positions of body parts with repsect to other body parts)
rlm@173:   in jMonkeyEngine3. Reads specially prepared blender files to
rlm@173:   automatically generate proprioceptive senses."
rlm@173:   (:use (cortex world util sense body))
rlm@173:   (:use clojure.contrib.def))
rlm@173:         
rlm@157: 
rlm@157: (cortex.import/mega-import-jme3)
rlm@157: 
rlm@173: (defvar 
rlm@173:   ^{:arglists '([creature])}
rlm@173:   joints
rlm@173:   (sense-nodes "joints")
rlm@173:   "Return the children of the creature's \"joints\" node.")
rlm@173: 
rlm@173: (defn right-handed?
rlm@173:   "true iff the three vectors form a right handed coordinate
rlm@173:   system. The three vectors do not have to be normalized or
rlm@173:   orthogonal."
rlm@173:   [vec1 vec2 vec3]
rlm@157:   (< 0 (.dot (.cross vec1 vec2) vec3)))
rlm@157: 
rlm@173: (defn absolute-angle
rlm@173:   "The angle between 'vec1 and 'vec2. Positive if the angle to get
rlm@173:   from 'vec1 to 'vec2 is counterclockwise around 'axis, and negative
rlm@173:   otherwise." 
rlm@173:   [vec1 vec2 axis]
rlm@157:   (let [angle (.angleBetween vec1 vec2)]
rlm@157:     (if (right-handed? vec1 vec2 axis)
rlm@157:       angle (- (* 2 Math/PI) angle))))
rlm@157: 
rlm@173: (defn proprioception-fn
rlm@173:   "Returns a function which returns proprioceptive sensory data when
rlm@173:   called inside a running simulation."
rlm@173:   [#^Node parts #^Node joint]
rlm@157:   (let [[obj-a obj-b] (joint-targets parts joint)
rlm@157:         joint-rot (.getWorldRotation joint)
rlm@157:         x0 (.mult joint-rot Vector3f/UNIT_X)
rlm@157:         y0 (.mult joint-rot Vector3f/UNIT_Y)
rlm@157:         z0 (.mult joint-rot Vector3f/UNIT_Z)]
rlm@157:     (println-repl "x:" x0)
rlm@157:     (println-repl "y:" y0)
rlm@157:     (println-repl "z:" z0)
rlm@157:     (println-repl "init-a:" (.getWorldRotation obj-a))
rlm@157:     (println-repl "init-b:" (.getWorldRotation obj-b))
rlm@157: 
rlm@157:     (fn []
rlm@157:       (let [rot-a (.clone (.getWorldRotation obj-a))
rlm@157:             rot-b (.clone (.getWorldRotation obj-b))
rlm@157:             x (.mult rot-a x0)
rlm@157:             y (.mult rot-a y0)
rlm@157:             z (.mult rot-a z0)
rlm@157: 
rlm@157:             X (.mult rot-b x0)
rlm@157:             Y (.mult rot-b y0)
rlm@157:             Z (.mult rot-b z0)
rlm@157:             heading  (Math/atan2 (.dot X z) (.dot X x))
rlm@157:             pitch  (Math/atan2 (.dot X y) (.dot X x))
rlm@157: 
rlm@157:             ;; rotate x-vector back to origin
rlm@157:             reverse
rlm@157:             (doto (Quaternion.)
rlm@157:               (.fromAngleAxis
rlm@157:                (.angleBetween X x)
rlm@157:                (let [cross (.normalize (.cross X x))]
rlm@157:                  (if (= 0 (.length cross)) y cross))))
rlm@157:             roll (absolute-angle (.mult reverse Y) y x)]
rlm@157:         [heading pitch roll]))))
rlm@157: 
rlm@173: (defn proprioception!
rlm@173:   "Endow the creature with the sense of proprioception. Returns a
rlm@173:    sequence of functions, one for each child of the \"joints\" node in
rlm@173:    the creature, which each report proprioceptive information about
rlm@173:    that joint."
rlm@157:   [#^Node creature]
rlm@157:   ;; extract the body's joints
rlm@173:   (let [senses (map (partial proprioception-fn creature)
rlm@173:                     (joints creature))]
rlm@157:     (fn []
rlm@157:       (map #(%) senses))))
rlm@157: 
rlm@157: 
rlm@157: 
rlm@157: 
rlm@157: 
rlm@157: #+end_src
rlm@157: 
rlm@157: 
rlm@157: 
rlm@157: 
rlm@157: 
rlm@157: 
rlm@157: 
rlm@157: 
rlm@157: * COMMENT generate source
rlm@157: #+begin_src clojure :tangle ../src/cortex/proprioception.clj
rlm@157: <<proprioception>>
rlm@157: #+end_src