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

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

 

 

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

 #+end_src