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@257: * Proprioception
rlm@257: 
rlm@257: Close your eyes, and touch your nose with your right index finger. How
rlm@257: did you do it? You could not see your hand, and neither your hand nor
rlm@257: your nose could use the sense of touch to guide the path of your hand.
rlm@257: There are no sound cues, and Taste and Smell certainly don't provide
rlm@257: any help. You know where your hand is without your other senses
rlm@257: because of Proprioception.
rlm@257: 
rlm@257: Humans can sometimes loose this sense through viral infections or
rlm@257: damage to the spinal cord or brain, and when they do, they loose the
rlm@257: ability to control their own bodies without looking directly at the
rlm@257: parts they want to move. In [[http://en.wikipedia.org/wiki/The_Man_Who_Mistook_His_Wife_for_a_Hat][The Man Who Mistook His Wife for a Hat]],
rlm@257: a woman named Christina looses this sense and has to learn how to move
rlm@257: by carefully watching her arms and legs. She describes proprioception
rlm@257: as the "eyes of the body, the way the body sees itself". 
rlm@257: 
rlm@257: Proprioception in humans is mediated by [[http://en.wikipedia.org/wiki/Articular_capsule][joint capsules]], [[http://en.wikipedia.org/wiki/Muscle_spindle][muscle
rlm@257: spindles]], and the [[http://en.wikipedia.org/wiki/Golgi_tendon_organ][Golgi tendon organs]]. These measure the relative
rlm@306: positions of each body part by monitoring muscle strain and length.
rlm@257: 
rlm@306: It's clear that this is a vital sense for fluid, graceful
rlm@306: movement. It's also particularly easy to implement in jMonkeyEngine.
rlm@257: 
rlm@257: My simulated proprioception calculates the relative angles of each
rlm@257: joint from the rest position defined in the blender file. This
rlm@257: simulates the muscle-spindles and joint capsules.  I will deal with
rlm@257: Golgi tendon organs, which calculate muscle strain, in the [[./movement.org][next post]].
rlm@257: 
rlm@257: * Helper Functions
rlm@257: 
rlm@273: =absolute-angle= calculates the angle between two vectors, relative to a
rlm@259: third axis vector. This angle is the number of radians you have to
rlm@259: move counterclockwise around the axis vector to get from the first to
rlm@259: the second vector. It is not commutative like a normal dot-product
rlm@259: angle is.
rlm@259: 
rlm@257: #+name: helpers
rlm@157: #+begin_src clojure
rlm@257: (in-ns 'cortex.proprioception)
rlm@157: 
rlm@173: (defn right-handed?
rlm@173:   "true iff the three vectors form a right handed coordinate
rlm@257:    system. The three vectors do not have to be normalized or
rlm@257:    orthogonal."
rlm@173:   [vec1 vec2 vec3]
rlm@157:   (< 0 (.dot (.cross vec1 vec2) vec3)))
rlm@157: 
rlm@173: (defn absolute-angle
rlm@259:   "The angle between 'vec1 and 'vec2 around 'axis. In the range 
rlm@259:    [0 (* 2 Math/PI)]."
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@257: #+end_src
rlm@157: 
rlm@259: #+begin_src clojure :exports both
rlm@259: (in-ns 'cortex.proprioception)
rlm@259: (absolute-angle  Vector3f/UNIT_X Vector3f/UNIT_Y Vector3f/UNIT_Z)
rlm@259: #+end_src
rlm@259: 
rlm@259: #+results:
rlm@259: : 1.5707964
rlm@259: 
rlm@259: #+begin_src clojure :exports both
rlm@259: (in-ns 'cortex.proprioception)
rlm@259: (absolute-angle 
rlm@259:  Vector3f/UNIT_X (.mult Vector3f/UNIT_Y (float -1)) Vector3f/UNIT_Z)
rlm@259: #+end_src
rlm@259: 
rlm@259: #+results:
rlm@259: : 4.7123889366733
rlm@259: 
rlm@258: * Proprioception Kernel
rlm@258: 
rlm@273: Given a joint, =proprioception-kernel= produces a function that
rlm@306: calculates the Euler angles between the the objects the joint
rlm@259: connects. 
rlm@259: 
rlm@257: #+name: proprioception
rlm@257: #+begin_src clojure
rlm@257: (defn proprioception-kernel
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:     (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@257:   (let [senses (map (partial proprioception-kernel creature)
rlm@173:                     (joints creature))]
rlm@157:     (fn []
rlm@157:       (map #(%) senses))))
rlm@257: #+end_src
rlm@175: 
rlm@259: 
rlm@273: =proprioception!= maps =proprioception-kernel= across all the
rlm@259: joints of the creature. It uses the same list of joints that
rlm@273: =cortex.body/joints= uses.
rlm@259: 
rlm@258: * Visualizing Proprioception
rlm@258: 
rlm@259: Proprioception has the lowest bandwidth of all the senses so far, and
rlm@259: it doesn't lend itself as readily to visual representation like
rlm@306: vision, hearing, or touch. This visualization code creates a "gauge"
rlm@259: to view each of the three relative angles along a circle.
rlm@259: 
rlm@257: #+name: visualize
rlm@257: #+begin_src clojure 
rlm@257: (in-ns 'cortex.proprioception)
rlm@175: 
rlm@175: (defn draw-sprite [image sprite x y color ]
rlm@175:   (dorun
rlm@175:    (for [[u v] sprite]
rlm@175:      (.setRGB image (+ u x) (+ v y) color))))
rlm@175: 
rlm@175: (defn view-angle
rlm@175:   "create a debug view of an angle"
rlm@175:   [color]
rlm@175:   (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
rlm@175:         previous (atom [25 25])
rlm@175:         sprite [[0 0] [0 1]
rlm@175:                 [0 -1] [-1 0] [1 0]]] 
rlm@175:     (fn [angle]
rlm@175:       (let [angle (float angle)]
rlm@175:         (let [position
rlm@175:               [(+ 25 (int (* 20 (Math/cos angle))))
rlm@175:                (+ 25 (int (* -20 (Math/sin angle))))]]
rlm@175:           (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)
rlm@175:           (draw-sprite image sprite (position 0) (position 1) color)
rlm@175:           (reset! previous position))
rlm@175:         image))))
rlm@175: 
rlm@190: (defn proprioception-display-kernel
rlm@190:   "Display proprioception angles in a BufferedImage"
rlm@190:   [[h p r]]
rlm@190:   (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
rlm@190:         previous-heading (atom [25 25])
rlm@190:         previous-pitch   (atom [25 25])
rlm@190:         previous-roll    (atom [25 25])
rlm@190:         
rlm@190:         heading-sprite [[0 0] [0 1] [0 -1] [-1 0] [1 0]]
rlm@190:         pitch-sprite   [[0 0] [0 1] [0 -1] [-1 0] [1 0]]
rlm@190:         roll-sprite    [[0 0] [0 1] [0 -1] [-1 0] [1 0]]
rlm@190:         draw-angle
rlm@190:         (fn [angle sprite previous color]
rlm@190:           (let [angle (float angle)]
rlm@190:             (let [position
rlm@190:                   [(+ 25 (int (* 20 (Math/cos angle))))
rlm@190:                    (+ 25 (int (* -20 (Math/sin angle))))]]
rlm@190:               (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)
rlm@190:               (draw-sprite image sprite (position 0) (position 1) color)
rlm@190:               (reset! previous position))
rlm@190:             image))]
rlm@190:     (dorun (map draw-angle
rlm@190:                 [h p r]
rlm@190:                 [heading-sprite pitch-sprite roll-sprite]
rlm@190:                 [previous-heading previous-pitch previous-roll]
rlm@190:                 [0xFF0000 0x00FF00 0xFFFFFF]))
rlm@190:     image))
rlm@190:                 
rlm@190: (defn view-proprioception
rlm@190:   "Creates a function which accepts a list of proprioceptive data and
rlm@190:    display each element of the list to the screen as an image."
rlm@175:   []
rlm@190:   (view-sense proprioception-display-kernel))
rlm@257: #+end_src
rlm@175: 
rlm@259: * Proprioception Test
rlm@259: This test does not use the worm, but instead uses two bars, bound
rlm@259: together by a point2point joint. One bar is fixed, and I control the
rlm@259: other bar from the keyboard. 
rlm@157: 
rlm@259: #+name: test-proprioception
rlm@206: #+begin_src clojure
rlm@259: (in-ns 'cortex.test.proprioception)
rlm@259: 
rlm@206: (defn test-proprioception
rlm@206:   "Testing proprioception:
rlm@321:    You should see two floating bars, and a display of pitch, yaw, and
rlm@321:    roll. The white dot measures pitch (spin around the long axis), the
rlm@321:    green dot measures yaw (in this case, rotation around a circle
rlm@321:    perpendicular to your line of view), and the red dot measures
rlm@321:    roll (rotation around a circle perlendicular to the the other two
rlm@321:    circles).
rlm@321: 
rlm@321:    Keys:
rlm@321:      r : rotate along long axis
rlm@321:      t : opposite direction of rotation as <r>
rlm@321:      
rlm@321:      f : rotate in field of view
rlm@321:      g : opposite direction of rotation as <f>
rlm@321: 
rlm@321:      v : rotate in final direction
rlm@321:      b : opposite direction of rotation as <v>"
rlm@321: 
rlm@259:   ([] (test-proprioception false))
rlm@259:   ([record?]
rlm@259:      (let [hand    (box 0.2 1 0.2 :position (Vector3f. 0 0 0)
rlm@259:                         :mass 0 :color ColorRGBA/Gray :name "hand")
rlm@259:            finger (box 0.2 1 0.2 :position (Vector3f. 0 2.4 0)
rlm@259:                        :mass 1
rlm@259:                        :color
rlm@259:                        (ColorRGBA. (/ 184 255) (/ 127 255) (/ 201 255) 1) 
rlm@259:                        :name "finger")
rlm@259:            joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow
rlm@259:                            :position (Vector3f. 0 1.2 0)
rlm@259:                            :rotation (doto (Quaternion.)
rlm@259:                                        (.fromAngleAxis
rlm@259:                                         (/ Math/PI 2)
rlm@259:                                         (Vector3f. 0 0 1)))
rlm@259:                            :physical? false)
rlm@259:            creature (nodify [hand finger joint-node])
rlm@259:            finger-control (.getControl finger RigidBodyControl)
rlm@259:            hand-control (.getControl hand RigidBodyControl)
rlm@259:            joint (joint-dispatch {:type :point} hand-control finger-control
rlm@259:                                  (Vector3f. 0 1.2  0)
rlm@259:                                  (Vector3f. 0 -1.2 0) nil)
rlm@206: 
rlm@259:            root (nodify [creature])
rlm@259:            prop (proprioception-kernel creature joint-node)
rlm@259:            prop-view (view-proprioception)]
rlm@259:        (.setCollisionGroup
rlm@259:         (.getControl hand RigidBodyControl)
rlm@259:         PhysicsCollisionObject/COLLISION_GROUP_NONE)
rlm@259:        (apply
rlm@259:         world
rlm@259:         (with-movement
rlm@259:           finger
rlm@259:           ["key-r" "key-t" "key-f" "key-g" "key-v" "key-b"]
rlm@259:           [1 1 10 10 10 10]
rlm@259:           [root
rlm@259:            standard-debug-controls
rlm@259:            (fn [world]
rlm@259:              (if record?
rlm@259:                (Capture/captureVideo
rlm@259:                 world
rlm@259:                 (File. "/home/r/proj/cortex/render/proprio/main-view")))
rlm@259:              (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
rlm@259:              (set-gravity world (Vector3f. 0 0 0))
rlm@259:              (enable-debug world)
rlm@259:              (light-up-everything world))
rlm@259:            (fn [_ _]
rlm@259:              (prop-view
rlm@259:               (list (prop))
rlm@259:               (if record?
rlm@259:                 (File. "/home/r/proj/cortex/render/proprio/proprio"))))])))))
rlm@259: #+end_src
rlm@206: 
rlm@259: #+results: test-proprioception
rlm@259: : #'cortex.test.proprioception/test-proprioception
rlm@259: 
rlm@259: * Video of Proprioception
rlm@259: 
rlm@259: #+begin_html
rlm@259: <div class="figure">
rlm@259: <center>
rlm@259: <video controls="controls" width="550">
rlm@259:   <source src="../video/test-proprioception.ogg" type="video/ogg"
rlm@259: 	  preload="none" poster="../images/aurellem-1280x480.png" />
rlm@259: </video>
rlm@309:  <br> <a href="http://youtu.be/JjdDmyM8b0w"> YouTube </a>
rlm@259: </center>
rlm@259: <p>Proprioception in a simple creature. The proprioceptive readout is
rlm@259:   in the upper left corner of the screen.</p>
rlm@259: </div>
rlm@259: #+end_html
rlm@259: 
rlm@259: ** Generating the Proprioception Video
rlm@259: #+name: magick6
rlm@259: #+begin_src clojure
rlm@259: (ns cortex.video.magick6
rlm@259:   (:import java.io.File)
rlm@316:   (:use clojure.java.shell))
rlm@259: 
rlm@259: (defn images [path]
rlm@259:   (sort (rest (file-seq (File. path)))))
rlm@259: 
rlm@259: (def base "/home/r/proj/cortex/render/proprio/")
rlm@259: 
rlm@259: (defn pics [file]
rlm@259:   (images (str base file)))
rlm@259: 
rlm@259: (defn combine-images []
rlm@259:   (let [main-view (pics "main-view")
rlm@259:         proprioception (pics "proprio/0")
rlm@259:         targets (map
rlm@259:                  #(File. (str base "out/" (format "%07d.png" %)))
rlm@259:                  (range 0 (count main-view)))]
rlm@259:     (dorun
rlm@259:      (pmap
rlm@259:       (comp
rlm@259:        (fn [[ main-view proprioception target]]
rlm@259:          (println target)
rlm@259:          (sh "convert"
rlm@259:              main-view 
rlm@259:              proprioception "-geometry" "+20+20" "-composite"
rlm@259:              target))
rlm@259:        (fn [& args] (map #(.getCanonicalPath %) args)))
rlm@259:        main-view proprioception targets))))
rlm@259: #+end_src
rlm@259: 
rlm@259: #+begin_src sh :results silent
rlm@259: cd ~/proj/cortex/render/proprio
rlm@259: ffmpeg -r 60 -i out/%07d.png -b:v 9000k -c:v libtheora \
rlm@259:        test-proprioception.ogg
rlm@258: #+end_src
rlm@206: 
rlm@258: * Headers
rlm@258: #+name: proprioception-header
rlm@258: #+begin_src clojure
rlm@258: (ns cortex.proprioception
rlm@258:   "Simulate the sense of proprioception (ability to detect the
rlm@306:   relative positions of body parts with respect to other body parts)
rlm@258:   in jMonkeyEngine3. Reads specially prepared blender files to
rlm@258:   automatically generate proprioceptive senses."
rlm@258:   (:use (cortex world util sense body))
rlm@258:   (:import com.jme3.scene.Node)
rlm@258:   (:import java.awt.image.BufferedImage)
rlm@258:   (:import (com.jme3.math Vector3f Quaternion)))
rlm@206: #+end_src
rlm@206: 
rlm@259: #+name: test-proprioception-header
rlm@259: #+begin_src clojure
rlm@259: (ns cortex.test.proprioception
rlm@283:   (:import (com.aurellem.capture Capture RatchetTimer))
rlm@283:   (:use (cortex util world proprioception body))
rlm@283:   (:import java.io.File)
rlm@283:   (:import com.jme3.bullet.control.RigidBodyControl)
rlm@283:   (:import com.jme3.bullet.collision.PhysicsCollisionObject)
rlm@283:   (:import (com.jme3.math Vector3f Quaternion ColorRGBA)))
rlm@259: #+end_src
rlm@259: 
rlm@259: * Source Listing
rlm@259:   - [[../src/cortex/proprioception.clj][cortex.proprioception]]
rlm@259:   - [[../src/cortex/test/touch.clj][cortex.test.proprioception]]
rlm@259:   - [[../src/cortex/video/magick6.clj][cortex.video.magick6]]
rlm@259: #+html: <ul> <li> <a href="../org/proprioception.org">This org file</a> </li> </ul>
rlm@259:   - [[http://hg.bortreb.com ][source-repository]]
rlm@259: 
rlm@259: * Next 
rlm@259: 
rlm@259: Next time, I'll give the Worm the power to [[./movement.org][move on it's own]].
rlm@259: 
rlm@206: 
rlm@157: * COMMENT generate source
rlm@157: #+begin_src clojure :tangle ../src/cortex/proprioception.clj
rlm@257: <<proprioception-header>>
rlm@257: <<helpers>>
rlm@157: <<proprioception>>
rlm@257: <<visualize>>
rlm@157: #+end_src
rlm@259: 
rlm@259: #+begin_src clojure :tangle ../src/cortex/test/proprioception.clj
rlm@259: <<test-proprioception-header>>
rlm@259: <<test-proprioception>>
rlm@259: #+end_src
rlm@259: 
rlm@259: #+begin_src clojure :tangle ../src/cortex/video/magick6.clj
rlm@259: <<magick6>>
rlm@259: #+end_src