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

 

 * Proprioception

 

 Close your eyes, and touch your nose with your right index finger. How

 did you do it? You could not see your hand, and neither your hand nor

 your nose could use the sense of touch to guide the path of your hand.

 There are no sound cues, and Taste and Smell certainly don't provide

 any help. You know where your hand is without your other senses

 because of Proprioception.

 

 Humans can sometimes loose this sense through viral infections or

 damage to the spinal cord or brain, and when they do, they loose the

 ability to control their own bodies without looking directly at the

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

 a woman named Christina looses this sense and has to learn how to move

 by carefully watching her arms and legs. She describes proprioception

 as the "eyes of the body, the way the body sees itself". 

 

 Proprioception in humans is mediated by [[http://en.wikipedia.org/wiki/Articular_capsule][joint capsules]], [[http://en.wikipedia.org/wiki/Muscle_spindle][muscle

 spindles]], and the [[http://en.wikipedia.org/wiki/Golgi_tendon_organ][Golgi tendon organs]]. These measure the relative

 positions of each pody part by monitoring muscle strain and length.

 

 It's clear that this is a vital sense for fulid, graceful

 movement. It's also particurally easy to implement in jMonkeyEngine.

 

 My simulated proprioception calculates the relative angles of each

 joint from the rest position defined in the blender file. This

 simulates the muscle-spindles and joint capsules.  I will deal with

 Golgi tendon organs, which calculate muscle strain, in the [[./movement.org][next post]].

 

 * Helper Functions

 

 =(absolute-angle)= calculates the angle between two vectors, relative to a

 third axis vector. This angle is the number of radians you have to

 move counterclockwise around the axis vector to get from the first to

 the second vector. It is not commutative like a normal dot-product

 angle is.

 

 #+name: helpers

 #+begin_src clojure

 (in-ns 'cortex.proprioception)

 

 (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 around 'axis. In the range 

    [0 (* 2 Math/PI)]."

   [vec1 vec2 axis]

   (let [angle (.angleBetween vec1 vec2)]

     (if (right-handed? vec1 vec2 axis)

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

 #+end_src

 

 #+begin_src clojure :exports both

 (in-ns 'cortex.proprioception)

 (absolute-angle  Vector3f/UNIT_X Vector3f/UNIT_Y Vector3f/UNIT_Z)

 #+end_src

 

 #+results:

 : 1.5707964

 

 #+begin_src clojure :exports both

 (in-ns 'cortex.proprioception)

 (absolute-angle 

  Vector3f/UNIT_X (.mult Vector3f/UNIT_Y (float -1)) Vector3f/UNIT_Z)

 #+end_src

 

 #+results:

 : 4.7123889366733

 

 * Proprioception Kernel

 

 Given a joint, =(proprioception-kernel)= produces a function that

 calculates the euler angles between the the objects the joint

 connects. 

 

 #+name: proprioception

 #+begin_src clojure

 (defn proprioception-kernel

   "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)]

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

                     (joints creature))]

     (fn []

       (map #(%) senses))))

 #+end_src

 

 

 =(proprioception!)= maps =(proprioception-kernel)= across all the

 joints of the creature. It uses the same list of joints that

 =(cortex.body/joints)= uses.

 

 * Visualizing Proprioception

 

 Proprioception has the lowest bandwidth of all the senses so far, and

 it doesn't lend itself as readily to visual representation like

 vision, hearing, or touch. This visualization code creates a "guage"

 to view each of the three relative angles along a circle.

 

 #+name: visualize

 #+begin_src clojure 

 (in-ns 'cortex.proprioception)

 

 (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-display-kernel

   "Display proprioception angles in a BufferedImage"

   [[h p r]]

   (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)

         previous-heading (atom [25 25])

         previous-pitch   (atom [25 25])

         previous-roll    (atom [25 25])

         

         heading-sprite [[0 0] [0 1] [0 -1] [-1 0] [1 0]]

         pitch-sprite   [[0 0] [0 1] [0 -1] [-1 0] [1 0]]

         roll-sprite    [[0 0] [0 1] [0 -1] [-1 0] [1 0]]

         draw-angle

         (fn [angle sprite previous color]

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

     (dorun (map draw-angle

                 [h p r]

                 [heading-sprite pitch-sprite roll-sprite]

                 [previous-heading previous-pitch previous-roll]

                 [0xFF0000 0x00FF00 0xFFFFFF]))

     image))

                 

 (defn view-proprioception

   "Creates a function which accepts a list of proprioceptive data and

    display each element of the list to the screen as an image."

   []

   (view-sense proprioception-display-kernel))

 #+end_src

 

 * Proprioception Test

 This test does not use the worm, but instead uses two bars, bound

 together by a point2point joint. One bar is fixed, and I control the

 other bar from the keyboard. 

 

 #+name: test-proprioception

 #+begin_src clojure

 (in-ns 'cortex.test.proprioception)

 

 (defn test-proprioception

   "Testing proprioception:

    You should see two foating bars, and a printout of pitch, yaw, and

    roll. Pressing key-r/key-t should move the blue bar up and down and

    change only the value of pitch. key-f/key-g moves it side to side

    and changes yaw. key-v/key-b will spin the blue segment clockwise

    and counterclockwise, and only affect roll."

   ([] (test-proprioception false))

   ([record?]

      (let [hand    (box 0.2 1 0.2 :position (Vector3f. 0 0 0)

                         :mass 0 :color ColorRGBA/Gray :name "hand")

            finger (box 0.2 1 0.2 :position (Vector3f. 0 2.4 0)

                        :mass 1

                        :color

                        (ColorRGBA. (/ 184 255) (/ 127 255) (/ 201 255) 1) 

                        :name "finger")

            joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow

                            :position (Vector3f. 0 1.2 0)

                            :rotation (doto (Quaternion.)

                                        (.fromAngleAxis

                                         (/ Math/PI 2)

                                         (Vector3f. 0 0 1)))

                            :physical? false)

            creature (nodify [hand finger joint-node])

            finger-control (.getControl finger RigidBodyControl)

            hand-control (.getControl hand RigidBodyControl)

            joint (joint-dispatch {:type :point} hand-control finger-control

                                  (Vector3f. 0 1.2  0)

                                  (Vector3f. 0 -1.2 0) nil)

 

            root (nodify [creature])

            prop (proprioception-kernel creature joint-node)

            prop-view (view-proprioception)]

        (.setCollisionGroup

         (.getControl hand RigidBodyControl)

         PhysicsCollisionObject/COLLISION_GROUP_NONE)

        (apply

         world

         (with-movement

           finger

           ["key-r" "key-t" "key-f" "key-g" "key-v" "key-b"]

           [1 1 10 10 10 10]

           [root

            standard-debug-controls

            (fn [world]

              (if record?

                (Capture/captureVideo

                 world

                 (File. "/home/r/proj/cortex/render/proprio/main-view")))

              (.setTimer world (com.aurellem.capture.RatchetTimer. 60))

              (set-gravity world (Vector3f. 0 0 0))

              (enable-debug world)

              (light-up-everything world))

            (fn [_ _]

              (prop-view

               (list (prop))

               (if record?

                 (File. "/home/r/proj/cortex/render/proprio/proprio"))))])))))

 #+end_src

 

 #+results: test-proprioception

 : #'cortex.test.proprioception/test-proprioception

 

 * Video of Proprioception

 

 #+begin_html

 <div class="figure">

 <center>

 <video controls="controls" width="550">

   <source src="../video/test-proprioception.ogg" type="video/ogg"

 	  preload="none" poster="../images/aurellem-1280x480.png" />

 </video>

 </center>

 <p>Proprioception in a simple creature. The proprioceptive readout is

   in the upper left corner of the screen.</p>

 </div>

 #+end_html

 

 ** Generating the Proprioception Video

 #+name: magick6

 #+begin_src clojure

 (ns cortex.video.magick6

   (:import java.io.File)

   (:use clojure.contrib.shell-out))

 

 (defn images [path]

   (sort (rest (file-seq (File. path)))))

 

 (def base "/home/r/proj/cortex/render/proprio/")

 

 (defn pics [file]

   (images (str base file)))

 

 (defn combine-images []

   (let [main-view (pics "main-view")

         proprioception (pics "proprio/0")

         targets (map

                  #(File. (str base "out/" (format "%07d.png" %)))

                  (range 0 (count main-view)))]

     (dorun

      (pmap

       (comp

        (fn [[ main-view proprioception target]]

          (println target)

          (sh "convert"

              main-view 

              proprioception "-geometry" "+20+20" "-composite"

              target))

        (fn [& args] (map #(.getCanonicalPath %) args)))

        main-view proprioception targets))))

 #+end_src

 

 #+begin_src sh :results silent

 cd ~/proj/cortex/render/proprio

 ffmpeg -r 60 -i out/%07d.png -b:v 9000k -c:v libtheora \

        test-proprioception.ogg

 #+end_src

 

 * Headers

 #+name: proprioception-header

 #+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 java.awt.image.BufferedImage)

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

 #+end_src

 

 #+name: test-proprioception-header

 #+begin_src clojure

 (ns cortex.test.proprioception

 (:import (com.aurellem.capture Capture RatchetTimer))

 (:use (cortex util world proprioception body))

 (:import java.io.File))

 (cortex.import/mega-import-jme3)

 #+end_src

 

 * Source Listing

   - [[../src/cortex/proprioception.clj][cortex.proprioception]]

   - [[../src/cortex/test/touch.clj][cortex.test.proprioception]]

   - [[../src/cortex/video/magick6.clj][cortex.video.magick6]]

 #+html: <ul> <li> <a href="../org/proprioception.org">This org file</a> </li> </ul>

   - [[http://hg.bortreb.com ][source-repository]]

 

 * Next 

 

 Next time, I'll give the Worm the power to [[./movement.org][move on it's own]].

 

 

 * COMMENT generate source

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

 <<proprioception-header>>

 <<helpers>>

 <<proprioception>>

 <<visualize>>

 #+end_src

 

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

 <<test-proprioception-header>>

 <<test-proprioception>>

 #+end_src

 

 #+begin_src clojure :tangle ../src/cortex/video/magick6.clj

 <<magick6>>

 #+end_src