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@174:   (:use clojure.contrib.def)
rlm@174:   (:import com.jme3.scene.Node)
rlm@190:   (:import java.awt.image.BufferedImage)
rlm@174:   (:import (com.jme3.math Vector3f Quaternion)))
rlm@157: 
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@175: 
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@190: 
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: 
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@190:   
rlm@190:   
rlm@175: 
rlm@157: #+end_src
rlm@157: 
rlm@206: #+name: test-body
rlm@206: #+begin_src clojure
rlm@206: 
rlm@206: 
rlm@206: (defn test-proprioception
rlm@206:   "Testing proprioception:
rlm@206:    You should see two foating bars, and a printout of pitch, yaw, and
rlm@206:    roll. Pressing key-r/key-t should move the blue bar up and down and
rlm@206:    change only the value of pitch. key-f/key-g moves it side to side
rlm@206:    and changes yaw. key-v/key-b will spin the blue segment clockwise
rlm@206:    and counterclockwise, and only affect roll."
rlm@206:   []
rlm@206:   (let [hand    (box 0.2 1 0.2 :position (Vector3f. 0 0 0)
rlm@206:                      :mass 0 :color ColorRGBA/Green :name "hand")
rlm@206:         finger (box 0.2 1 0.2 :position (Vector3f. 0 2.4 0)
rlm@206:                     :mass 1 :color ColorRGBA/Red :name "finger")
rlm@206:         joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow
rlm@206:                         :position (Vector3f. 0 1.2 0)
rlm@206:                         :rotation (doto (Quaternion.)
rlm@206:                                     (.fromAngleAxis
rlm@206:                                      (/ Math/PI 2)
rlm@206:                                      (Vector3f. 0 0 1)))
rlm@206:                         :physical? false)
rlm@206:         joint (join-at-point hand finger (Vector3f. 0 1.2 0 ))
rlm@206:         creature (nodify [hand finger joint-node])
rlm@206:         finger-control (.getControl finger RigidBodyControl)
rlm@206:         hand-control (.getControl hand RigidBodyControl)]
rlm@206:     
rlm@206: 
rlm@206:     (let
rlm@206:         ;; *******************************************
rlm@206:                 
rlm@206:         [floor   (box 10 10 10 :position (Vector3f. 0 -15 0)
rlm@206:                      :mass 0 :color ColorRGBA/Gray)
rlm@206:         
rlm@206:         root (nodify [creature floor])
rlm@206:         prop (joint-proprioception creature joint-node)
rlm@206:         prop-view (proprioception-debug-window)
rlm@206:         
rlm@206:         controls
rlm@206:         (merge standard-debug-controls
rlm@206:                {"key-o"
rlm@206:                 (fn [_ _] (.setEnabled finger-control true))
rlm@206:                 "key-p"
rlm@206:                 (fn [_ _] (.setEnabled finger-control false))
rlm@206:                 "key-k"
rlm@206:                 (fn [_ _] (.setEnabled hand-control true))
rlm@206:                 "key-l"
rlm@206:                 (fn [_ _] (.setEnabled hand-control false))
rlm@206:                 "key-i"
rlm@206:                 (fn [world _] (set-gravity world (Vector3f. 0 0 0)))
rlm@206:                 "key-period"
rlm@206:                 (fn [world _]
rlm@206:                   (.setEnabled finger-control false)
rlm@206:                   (.setEnabled hand-control false)
rlm@206:                   (.rotate creature (doto (Quaternion.)
rlm@206:                                       (.fromAngleAxis
rlm@206:                                        (float (/ Math/PI 15))
rlm@206:                                        (Vector3f. 0 0 -1))))
rlm@206:                                               
rlm@206:                   (.setEnabled finger-control true)
rlm@206:                   (.setEnabled hand-control true)
rlm@206:                   (set-gravity world (Vector3f. 0 0 0))
rlm@206:                   )
rlm@206:                 
rlm@206:                   
rlm@206:                 }
rlm@206:                )
rlm@206: 
rlm@206:         ]
rlm@206:     (comment
rlm@206:       (.setCollisionGroup
rlm@206:        (.getControl hand RigidBodyControl)
rlm@206:        PhysicsCollisionObject/COLLISION_GROUP_NONE)
rlm@206:       )
rlm@206:     (apply
rlm@206:      world
rlm@206:      (with-movement
rlm@206:        hand
rlm@206:        ["key-y" "key-u" "key-h" "key-j" "key-n" "key-m"]
rlm@206:        [10 10 10 10 1 1]
rlm@206:        (with-movement
rlm@206:          finger
rlm@206:          ["key-r" "key-t" "key-f" "key-g" "key-v" "key-b"]
rlm@206:          [1 1 10 10 10 10]
rlm@206:          [root
rlm@206:           controls
rlm@206:           (fn [world]
rlm@206:             (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
rlm@206:             (set-gravity world (Vector3f. 0 0 0))
rlm@206:             (light-up-everything world))
rlm@206:           (fn [_ _] (prop-view (list (prop))))]))))))
rlm@206: 
rlm@206: #+end_src
rlm@206: 
rlm@206: 
rlm@157: * COMMENT generate source
rlm@157: #+begin_src clojure :tangle ../src/cortex/proprioception.clj
rlm@157: <<proprioception>>
rlm@157: #+end_src