Mercurial > cortex

annotate org/proprioception.org @ 258:f4b67005b702

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
reorganized proprioception
author Robert McIntyre <rlm@mit.edu>
date Tue, 14 Feb 2012 00:39:16 -0700
parents 5d7961d7fded
children 7cac5ef852e3
rev   line source
rlm@157 1 #+title: The Sense of Proprioception
rlm@157 2 #+author: Robert McIntyre
rlm@157 3 #+email: rlm@mit.edu
rlm@157 4 #+description: proprioception for simulated creatures
rlm@157 5 #+keywords: simulation, jMonkeyEngine3, clojure
rlm@157 6 #+SETUPFILE: ../../aurellem/org/setup.org
rlm@157 7 #+INCLUDE: ../../aurellem/org/level-0.org
rlm@157 8
rlm@257 9 * Proprioception
rlm@257 10
rlm@257 11 Close your eyes, and touch your nose with your right index finger. How
rlm@257 12 did you do it? You could not see your hand, and neither your hand nor
rlm@257 13 your nose could use the sense of touch to guide the path of your hand.
rlm@257 14 There are no sound cues, and Taste and Smell certainly don't provide
rlm@257 15 any help. You know where your hand is without your other senses
rlm@257 16 because of Proprioception.
rlm@257 17
rlm@257 18 Humans can sometimes loose this sense through viral infections or
rlm@257 19 damage to the spinal cord or brain, and when they do, they loose the
rlm@257 20 ability to control their own bodies without looking directly at the
rlm@257 21 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 22 a woman named Christina looses this sense and has to learn how to move
rlm@257 23 by carefully watching her arms and legs. She describes proprioception
rlm@257 24 as the "eyes of the body, the way the body sees itself".
rlm@257 25
rlm@257 26 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 27 spindles]], and the [[http://en.wikipedia.org/wiki/Golgi_tendon_organ][Golgi tendon organs]]. These measure the relative
rlm@257 28 positions of each pody part by monitoring muscle strain and length.
rlm@257 29
rlm@257 30 It's clear that this is a vital sense for fulid, graceful
rlm@257 31 movement. It's also particurally easy to implement in jMonkeyEngine.
rlm@257 32
rlm@257 33 My simulated proprioception calculates the relative angles of each
rlm@257 34 joint from the rest position defined in the blender file. This
rlm@257 35 simulates the muscle-spindles and joint capsules. I will deal with
rlm@257 36 Golgi tendon organs, which calculate muscle strain, in the [[./movement.org][next post]].
rlm@257 37
rlm@257 38 * Helper Functions
rlm@257 39
rlm@257 40 #+name: helpers
rlm@157 41 #+begin_src clojure
rlm@257 42 (in-ns 'cortex.proprioception)
rlm@157 43
rlm@173 44 (defn right-handed?
rlm@173 45 "true iff the three vectors form a right handed coordinate
rlm@257 46 system. The three vectors do not have to be normalized or
rlm@257 47 orthogonal."
rlm@173 48 [vec1 vec2 vec3]
rlm@157 49 (< 0 (.dot (.cross vec1 vec2) vec3)))
rlm@157 50
rlm@173 51 (defn absolute-angle
rlm@173 52 "The angle between 'vec1 and 'vec2. Positive if the angle to get
rlm@173 53 from 'vec1 to 'vec2 is counterclockwise around 'axis, and negative
rlm@173 54 otherwise."
rlm@173 55 [vec1 vec2 axis]
rlm@157 56 (let [angle (.angleBetween vec1 vec2)]
rlm@157 57 (if (right-handed? vec1 vec2 axis)
rlm@157 58 angle (- (* 2 Math/PI) angle))))
rlm@257 59 #+end_src
rlm@157 60
rlm@258 61 * Proprioception Kernel
rlm@258 62
rlm@257 63 #+name: proprioception
rlm@257 64 #+begin_src clojure
rlm@257 65 (defn proprioception-kernel
rlm@173 66 "Returns a function which returns proprioceptive sensory data when
rlm@173 67 called inside a running simulation."
rlm@173 68 [#^Node parts #^Node joint]
rlm@157 69 (let [[obj-a obj-b] (joint-targets parts joint)
rlm@157 70 joint-rot (.getWorldRotation joint)
rlm@157 71 x0 (.mult joint-rot Vector3f/UNIT_X)
rlm@157 72 y0 (.mult joint-rot Vector3f/UNIT_Y)
rlm@157 73 z0 (.mult joint-rot Vector3f/UNIT_Z)]
rlm@157 74 (println-repl "x:" x0)
rlm@157 75 (println-repl "y:" y0)
rlm@157 76 (println-repl "z:" z0)
rlm@157 77 (println-repl "init-a:" (.getWorldRotation obj-a))
rlm@157 78 (println-repl "init-b:" (.getWorldRotation obj-b))
rlm@157 79
rlm@157 80 (fn []
rlm@157 81 (let [rot-a (.clone (.getWorldRotation obj-a))
rlm@157 82 rot-b (.clone (.getWorldRotation obj-b))
rlm@157 83 x (.mult rot-a x0)
rlm@157 84 y (.mult rot-a y0)
rlm@157 85 z (.mult rot-a z0)
rlm@157 86
rlm@157 87 X (.mult rot-b x0)
rlm@157 88 Y (.mult rot-b y0)
rlm@157 89 Z (.mult rot-b z0)
rlm@157 90 heading (Math/atan2 (.dot X z) (.dot X x))
rlm@157 91 pitch (Math/atan2 (.dot X y) (.dot X x))
rlm@157 92
rlm@157 93 ;; rotate x-vector back to origin
rlm@157 94 reverse
rlm@157 95 (doto (Quaternion.)
rlm@157 96 (.fromAngleAxis
rlm@157 97 (.angleBetween X x)
rlm@157 98 (let [cross (.normalize (.cross X x))]
rlm@157 99 (if (= 0 (.length cross)) y cross))))
rlm@157 100 roll (absolute-angle (.mult reverse Y) y x)]
rlm@157 101 [heading pitch roll]))))
rlm@157 102
rlm@173 103 (defn proprioception!
rlm@173 104 "Endow the creature with the sense of proprioception. Returns a
rlm@173 105 sequence of functions, one for each child of the \"joints\" node in
rlm@173 106 the creature, which each report proprioceptive information about
rlm@173 107 that joint."
rlm@157 108 [#^Node creature]
rlm@157 109 ;; extract the body's joints
rlm@257 110 (let [senses (map (partial proprioception-kernel creature)
rlm@173 111 (joints creature))]
rlm@157 112 (fn []
rlm@157 113 (map #(%) senses))))
rlm@257 114 #+end_src
rlm@175 115
rlm@258 116 * Visualizing Proprioception
rlm@258 117
rlm@257 118 #+name: visualize
rlm@257 119 #+begin_src clojure
rlm@257 120 (in-ns 'cortex.proprioception)
rlm@175 121
rlm@175 122 (defn draw-sprite [image sprite x y color ]
rlm@175 123 (dorun
rlm@175 124 (for [[u v] sprite]
rlm@175 125 (.setRGB image (+ u x) (+ v y) color))))
rlm@175 126
rlm@175 127 (defn view-angle
rlm@175 128 "create a debug view of an angle"
rlm@175 129 [color]
rlm@175 130 (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
rlm@175 131 previous (atom [25 25])
rlm@175 132 sprite [[0 0] [0 1]
rlm@175 133 [0 -1] [-1 0] [1 0]]]
rlm@175 134 (fn [angle]
rlm@175 135 (let [angle (float angle)]
rlm@175 136 (let [position
rlm@175 137 [(+ 25 (int (* 20 (Math/cos angle))))
rlm@175 138 (+ 25 (int (* -20 (Math/sin angle))))]]
rlm@175 139 (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)
rlm@175 140 (draw-sprite image sprite (position 0) (position 1) color)
rlm@175 141 (reset! previous position))
rlm@175 142 image))))
rlm@175 143
rlm@190 144
rlm@190 145 (defn proprioception-display-kernel
rlm@190 146 "Display proprioception angles in a BufferedImage"
rlm@190 147 [[h p r]]
rlm@190 148 (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
rlm@190 149 previous-heading (atom [25 25])
rlm@190 150 previous-pitch (atom [25 25])
rlm@190 151 previous-roll (atom [25 25])
rlm@190 152
rlm@190 153 heading-sprite [[0 0] [0 1] [0 -1] [-1 0] [1 0]]
rlm@190 154 pitch-sprite [[0 0] [0 1] [0 -1] [-1 0] [1 0]]
rlm@190 155 roll-sprite [[0 0] [0 1] [0 -1] [-1 0] [1 0]]
rlm@190 156 draw-angle
rlm@190 157 (fn [angle sprite previous color]
rlm@190 158 (let [angle (float angle)]
rlm@190 159 (let [position
rlm@190 160 [(+ 25 (int (* 20 (Math/cos angle))))
rlm@190 161 (+ 25 (int (* -20 (Math/sin angle))))]]
rlm@190 162 (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)
rlm@190 163 (draw-sprite image sprite (position 0) (position 1) color)
rlm@190 164 (reset! previous position))
rlm@190 165 image))]
rlm@190 166 (dorun (map draw-angle
rlm@190 167 [h p r]
rlm@190 168 [heading-sprite pitch-sprite roll-sprite]
rlm@190 169 [previous-heading previous-pitch previous-roll]
rlm@190 170 [0xFF0000 0x00FF00 0xFFFFFF]))
rlm@190 171 image))
rlm@190 172
rlm@190 173 (defn view-proprioception
rlm@190 174 "Creates a function which accepts a list of proprioceptive data and
rlm@190 175 display each element of the list to the screen as an image."
rlm@175 176 []
rlm@190 177 (view-sense proprioception-display-kernel))
rlm@257 178 #+end_src
rlm@175 179
rlm@258 180 * Demonstration of Proprioception
rlm@157 181
rlm@206 182 #+name: test-body
rlm@206 183 #+begin_src clojure
rlm@206 184 (defn test-proprioception
rlm@206 185 "Testing proprioception:
rlm@206 186 You should see two foating bars, and a printout of pitch, yaw, and
rlm@206 187 roll. Pressing key-r/key-t should move the blue bar up and down and
rlm@206 188 change only the value of pitch. key-f/key-g moves it side to side
rlm@206 189 and changes yaw. key-v/key-b will spin the blue segment clockwise
rlm@206 190 and counterclockwise, and only affect roll."
rlm@206 191 []
rlm@206 192 (let [hand (box 0.2 1 0.2 :position (Vector3f. 0 0 0)
rlm@206 193 :mass 0 :color ColorRGBA/Green :name "hand")
rlm@206 194 finger (box 0.2 1 0.2 :position (Vector3f. 0 2.4 0)
rlm@206 195 :mass 1 :color ColorRGBA/Red :name "finger")
rlm@206 196 joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow
rlm@206 197 :position (Vector3f. 0 1.2 0)
rlm@206 198 :rotation (doto (Quaternion.)
rlm@206 199 (.fromAngleAxis
rlm@206 200 (/ Math/PI 2)
rlm@206 201 (Vector3f. 0 0 1)))
rlm@206 202 :physical? false)
rlm@206 203 joint (join-at-point hand finger (Vector3f. 0 1.2 0 ))
rlm@206 204 creature (nodify [hand finger joint-node])
rlm@206 205 finger-control (.getControl finger RigidBodyControl)
rlm@206 206 hand-control (.getControl hand RigidBodyControl)]
rlm@206 207
rlm@206 208
rlm@206 209 (let
rlm@206 210 ;; *******************************************
rlm@206 211
rlm@206 212 [floor (box 10 10 10 :position (Vector3f. 0 -15 0)
rlm@206 213 :mass 0 :color ColorRGBA/Gray)
rlm@206 214
rlm@206 215 root (nodify [creature floor])
rlm@206 216 prop (joint-proprioception creature joint-node)
rlm@206 217 prop-view (proprioception-debug-window)
rlm@206 218
rlm@206 219 controls
rlm@206 220 (merge standard-debug-controls
rlm@206 221 {"key-o"
rlm@206 222 (fn [_ _] (.setEnabled finger-control true))
rlm@206 223 "key-p"
rlm@206 224 (fn [_ _] (.setEnabled finger-control false))
rlm@206 225 "key-k"
rlm@206 226 (fn [_ _] (.setEnabled hand-control true))
rlm@206 227 "key-l"
rlm@206 228 (fn [_ _] (.setEnabled hand-control false))
rlm@206 229 "key-i"
rlm@206 230 (fn [world _] (set-gravity world (Vector3f. 0 0 0)))
rlm@206 231 "key-period"
rlm@206 232 (fn [world _]
rlm@206 233 (.setEnabled finger-control false)
rlm@206 234 (.setEnabled hand-control false)
rlm@206 235 (.rotate creature (doto (Quaternion.)
rlm@206 236 (.fromAngleAxis
rlm@206 237 (float (/ Math/PI 15))
rlm@206 238 (Vector3f. 0 0 -1))))
rlm@206 239
rlm@206 240 (.setEnabled finger-control true)
rlm@206 241 (.setEnabled hand-control true)
rlm@206 242 (set-gravity world (Vector3f. 0 0 0))
rlm@206 243 )
rlm@206 244
rlm@206 245
rlm@206 246 }
rlm@206 247 )
rlm@206 248
rlm@206 249 ]
rlm@206 250 (comment
rlm@206 251 (.setCollisionGroup
rlm@206 252 (.getControl hand RigidBodyControl)
rlm@206 253 PhysicsCollisionObject/COLLISION_GROUP_NONE)
rlm@206 254 )
rlm@206 255 (apply
rlm@206 256 world
rlm@206 257 (with-movement
rlm@206 258 hand
rlm@206 259 ["key-y" "key-u" "key-h" "key-j" "key-n" "key-m"]
rlm@206 260 [10 10 10 10 1 1]
rlm@206 261 (with-movement
rlm@206 262 finger
rlm@206 263 ["key-r" "key-t" "key-f" "key-g" "key-v" "key-b"]
rlm@206 264 [1 1 10 10 10 10]
rlm@206 265 [root
rlm@206 266 controls
rlm@206 267 (fn [world]
rlm@206 268 (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
rlm@206 269 (set-gravity world (Vector3f. 0 0 0))
rlm@206 270 (light-up-everything world))
rlm@206 271 (fn [_ _] (prop-view (list (prop))))]))))))
rlm@258 272 #+end_src
rlm@206 273
rlm@258 274 * Headers
rlm@258 275 #+name: proprioception-header
rlm@258 276 #+begin_src clojure
rlm@258 277 (ns cortex.proprioception
rlm@258 278 "Simulate the sense of proprioception (ability to detect the
rlm@258 279 relative positions of body parts with repsect to other body parts)
rlm@258 280 in jMonkeyEngine3. Reads specially prepared blender files to
rlm@258 281 automatically generate proprioceptive senses."
rlm@258 282 (:use (cortex world util sense body))
rlm@258 283 (:use clojure.contrib.def)
rlm@258 284 (:import com.jme3.scene.Node)
rlm@258 285 (:import java.awt.image.BufferedImage)
rlm@258 286 (:import (com.jme3.math Vector3f Quaternion)))
rlm@206 287 #+end_src
rlm@206 288
rlm@206 289
rlm@157 290 * COMMENT generate source
rlm@157 291 #+begin_src clojure :tangle ../src/cortex/proprioception.clj
rlm@257 292 <<proprioception-header>>
rlm@257 293 <<helpers>>
rlm@157 294 <<proprioception>>
rlm@257 295 <<visualize>>
rlm@157 296 #+end_src