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

 

 

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

 #+end_src

 

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

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

                     (joints creature))]

     (fn []

       (map #(%) senses))))

 #+end_src

 

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

 

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

 #+begin_src clojure

 (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."

   []

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

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

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

                     :mass 1 :color ColorRGBA/Red :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)

         joint (join-at-point hand finger (Vector3f. 0 1.2 0 ))

         creature (nodify [hand finger joint-node])

         finger-control (.getControl finger RigidBodyControl)

         hand-control (.getControl hand RigidBodyControl)]

     

 

     (let

         ;; *******************************************

                 

         [floor   (box 10 10 10 :position (Vector3f. 0 -15 0)

                      :mass 0 :color ColorRGBA/Gray)

         

         root (nodify [creature floor])

         prop (joint-proprioception creature joint-node)

         prop-view (proprioception-debug-window)

         

         controls

         (merge standard-debug-controls

                {"key-o"

                 (fn [_ _] (.setEnabled finger-control true))

                 "key-p"

                 (fn [_ _] (.setEnabled finger-control false))

                 "key-k"

                 (fn [_ _] (.setEnabled hand-control true))

                 "key-l"

                 (fn [_ _] (.setEnabled hand-control false))

                 "key-i"

                 (fn [world _] (set-gravity world (Vector3f. 0 0 0)))

                 "key-period"

                 (fn [world _]

                   (.setEnabled finger-control false)

                   (.setEnabled hand-control false)

                   (.rotate creature (doto (Quaternion.)

                                       (.fromAngleAxis

                                        (float (/ Math/PI 15))

                                        (Vector3f. 0 0 -1))))

                                               

                   (.setEnabled finger-control true)

                   (.setEnabled hand-control true)

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

                   )

                 

                   

                 }

                )

 

         ]

     (comment

       (.setCollisionGroup

        (.getControl hand RigidBodyControl)

        PhysicsCollisionObject/COLLISION_GROUP_NONE)

       )

     (apply

      world

      (with-movement

        hand

        ["key-y" "key-u" "key-h" "key-j" "key-n" "key-m"]

        [10 10 10 10 1 1]

        (with-movement

          finger

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

          [1 1 10 10 10 10]

          [root

           controls

           (fn [world]

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

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

             (light-up-everything world))

           (fn [_ _] (prop-view (list (prop))))]))))))

 

 #+end_src

 

 

 * COMMENT generate source

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

 <<proprioception-header>>

 <<helpers>>

 <<proprioception>>

 <<visualize>>

 #+end_src