Mercurial > cortex

annotate org/proprioception.org @ 215:f283c62bd212

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
fixed long standing problem with orientation of eyes in blender, fleshed out text in vision.org
author Robert McIntyre <rlm@mit.edu>
date Fri, 10 Feb 2012 02:19:24 -0700
parents df46a609fed9
children 5d7961d7fded
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@157 9 #+name: proprioception
rlm@157 10 #+begin_src clojure
rlm@157 11 (ns cortex.proprioception
rlm@173 12 "Simulate the sense of proprioception (ability to detect the
rlm@173 13 relative positions of body parts with repsect to other body parts)
rlm@173 14 in jMonkeyEngine3. Reads specially prepared blender files to
rlm@173 15 automatically generate proprioceptive senses."
rlm@173 16 (:use (cortex world util sense body))
rlm@174 17 (:use clojure.contrib.def)
rlm@174 18 (:import com.jme3.scene.Node)
rlm@190 19 (:import java.awt.image.BufferedImage)
rlm@174 20 (:import (com.jme3.math Vector3f Quaternion)))
rlm@157 21
rlm@173 22 (defn right-handed?
rlm@173 23 "true iff the three vectors form a right handed coordinate
rlm@173 24 system. The three vectors do not have to be normalized or
rlm@173 25 orthogonal."
rlm@173 26 [vec1 vec2 vec3]
rlm@157 27 (< 0 (.dot (.cross vec1 vec2) vec3)))
rlm@157 28
rlm@173 29 (defn absolute-angle
rlm@173 30 "The angle between 'vec1 and 'vec2. Positive if the angle to get
rlm@173 31 from 'vec1 to 'vec2 is counterclockwise around 'axis, and negative
rlm@173 32 otherwise."
rlm@173 33 [vec1 vec2 axis]
rlm@157 34 (let [angle (.angleBetween vec1 vec2)]
rlm@157 35 (if (right-handed? vec1 vec2 axis)
rlm@157 36 angle (- (* 2 Math/PI) angle))))
rlm@157 37
rlm@173 38 (defn proprioception-fn
rlm@173 39 "Returns a function which returns proprioceptive sensory data when
rlm@173 40 called inside a running simulation."
rlm@173 41 [#^Node parts #^Node joint]
rlm@157 42 (let [[obj-a obj-b] (joint-targets parts joint)
rlm@157 43 joint-rot (.getWorldRotation joint)
rlm@157 44 x0 (.mult joint-rot Vector3f/UNIT_X)
rlm@157 45 y0 (.mult joint-rot Vector3f/UNIT_Y)
rlm@157 46 z0 (.mult joint-rot Vector3f/UNIT_Z)]
rlm@157 47 (println-repl "x:" x0)
rlm@157 48 (println-repl "y:" y0)
rlm@157 49 (println-repl "z:" z0)
rlm@157 50 (println-repl "init-a:" (.getWorldRotation obj-a))
rlm@157 51 (println-repl "init-b:" (.getWorldRotation obj-b))
rlm@157 52
rlm@157 53 (fn []
rlm@157 54 (let [rot-a (.clone (.getWorldRotation obj-a))
rlm@157 55 rot-b (.clone (.getWorldRotation obj-b))
rlm@157 56 x (.mult rot-a x0)
rlm@157 57 y (.mult rot-a y0)
rlm@157 58 z (.mult rot-a z0)
rlm@157 59
rlm@157 60 X (.mult rot-b x0)
rlm@157 61 Y (.mult rot-b y0)
rlm@157 62 Z (.mult rot-b z0)
rlm@157 63 heading (Math/atan2 (.dot X z) (.dot X x))
rlm@157 64 pitch (Math/atan2 (.dot X y) (.dot X x))
rlm@157 65
rlm@157 66 ;; rotate x-vector back to origin
rlm@157 67 reverse
rlm@157 68 (doto (Quaternion.)
rlm@157 69 (.fromAngleAxis
rlm@157 70 (.angleBetween X x)
rlm@157 71 (let [cross (.normalize (.cross X x))]
rlm@157 72 (if (= 0 (.length cross)) y cross))))
rlm@157 73 roll (absolute-angle (.mult reverse Y) y x)]
rlm@157 74 [heading pitch roll]))))
rlm@157 75
rlm@173 76 (defn proprioception!
rlm@173 77 "Endow the creature with the sense of proprioception. Returns a
rlm@173 78 sequence of functions, one for each child of the \"joints\" node in
rlm@173 79 the creature, which each report proprioceptive information about
rlm@173 80 that joint."
rlm@157 81 [#^Node creature]
rlm@157 82 ;; extract the body's joints
rlm@173 83 (let [senses (map (partial proprioception-fn creature)
rlm@173 84 (joints creature))]
rlm@157 85 (fn []
rlm@157 86 (map #(%) senses))))
rlm@175 87
rlm@175 88
rlm@175 89 (defn draw-sprite [image sprite x y color ]
rlm@175 90 (dorun
rlm@175 91 (for [[u v] sprite]
rlm@175 92 (.setRGB image (+ u x) (+ v y) color))))
rlm@175 93
rlm@190 94
rlm@175 95 (defn view-angle
rlm@175 96 "create a debug view of an angle"
rlm@175 97 [color]
rlm@175 98 (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
rlm@175 99 previous (atom [25 25])
rlm@175 100 sprite [[0 0] [0 1]
rlm@175 101 [0 -1] [-1 0] [1 0]]]
rlm@175 102 (fn [angle]
rlm@175 103 (let [angle (float angle)]
rlm@175 104 (let [position
rlm@175 105 [(+ 25 (int (* 20 (Math/cos angle))))
rlm@175 106 (+ 25 (int (* -20 (Math/sin angle))))]]
rlm@175 107 (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)
rlm@175 108 (draw-sprite image sprite (position 0) (position 1) color)
rlm@175 109 (reset! previous position))
rlm@175 110 image))))
rlm@175 111
rlm@190 112
rlm@190 113 (defn proprioception-display-kernel
rlm@190 114 "Display proprioception angles in a BufferedImage"
rlm@190 115 [[h p r]]
rlm@190 116 (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
rlm@190 117 previous-heading (atom [25 25])
rlm@190 118 previous-pitch (atom [25 25])
rlm@190 119 previous-roll (atom [25 25])
rlm@190 120
rlm@190 121 heading-sprite [[0 0] [0 1] [0 -1] [-1 0] [1 0]]
rlm@190 122 pitch-sprite [[0 0] [0 1] [0 -1] [-1 0] [1 0]]
rlm@190 123 roll-sprite [[0 0] [0 1] [0 -1] [-1 0] [1 0]]
rlm@190 124 draw-angle
rlm@190 125 (fn [angle sprite previous color]
rlm@190 126 (let [angle (float angle)]
rlm@190 127 (let [position
rlm@190 128 [(+ 25 (int (* 20 (Math/cos angle))))
rlm@190 129 (+ 25 (int (* -20 (Math/sin angle))))]]
rlm@190 130 (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)
rlm@190 131 (draw-sprite image sprite (position 0) (position 1) color)
rlm@190 132 (reset! previous position))
rlm@190 133 image))]
rlm@190 134 (dorun (map draw-angle
rlm@190 135 [h p r]
rlm@190 136 [heading-sprite pitch-sprite roll-sprite]
rlm@190 137 [previous-heading previous-pitch previous-roll]
rlm@190 138 [0xFF0000 0x00FF00 0xFFFFFF]))
rlm@190 139 image))
rlm@190 140
rlm@190 141 (defn view-proprioception
rlm@190 142 "Creates a function which accepts a list of proprioceptive data and
rlm@190 143 display each element of the list to the screen as an image."
rlm@175 144 []
rlm@190 145 (view-sense proprioception-display-kernel))
rlm@190 146
rlm@190 147
rlm@175 148
rlm@157 149 #+end_src
rlm@157 150
rlm@206 151 #+name: test-body
rlm@206 152 #+begin_src clojure
rlm@206 153
rlm@206 154
rlm@206 155 (defn test-proprioception
rlm@206 156 "Testing proprioception:
rlm@206 157 You should see two foating bars, and a printout of pitch, yaw, and
rlm@206 158 roll. Pressing key-r/key-t should move the blue bar up and down and
rlm@206 159 change only the value of pitch. key-f/key-g moves it side to side
rlm@206 160 and changes yaw. key-v/key-b will spin the blue segment clockwise
rlm@206 161 and counterclockwise, and only affect roll."
rlm@206 162 []
rlm@206 163 (let [hand (box 0.2 1 0.2 :position (Vector3f. 0 0 0)
rlm@206 164 :mass 0 :color ColorRGBA/Green :name "hand")
rlm@206 165 finger (box 0.2 1 0.2 :position (Vector3f. 0 2.4 0)
rlm@206 166 :mass 1 :color ColorRGBA/Red :name "finger")
rlm@206 167 joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow
rlm@206 168 :position (Vector3f. 0 1.2 0)
rlm@206 169 :rotation (doto (Quaternion.)
rlm@206 170 (.fromAngleAxis
rlm@206 171 (/ Math/PI 2)
rlm@206 172 (Vector3f. 0 0 1)))
rlm@206 173 :physical? false)
rlm@206 174 joint (join-at-point hand finger (Vector3f. 0 1.2 0 ))
rlm@206 175 creature (nodify [hand finger joint-node])
rlm@206 176 finger-control (.getControl finger RigidBodyControl)
rlm@206 177 hand-control (.getControl hand RigidBodyControl)]
rlm@206 178
rlm@206 179
rlm@206 180 (let
rlm@206 181 ;; *******************************************
rlm@206 182
rlm@206 183 [floor (box 10 10 10 :position (Vector3f. 0 -15 0)
rlm@206 184 :mass 0 :color ColorRGBA/Gray)
rlm@206 185
rlm@206 186 root (nodify [creature floor])
rlm@206 187 prop (joint-proprioception creature joint-node)
rlm@206 188 prop-view (proprioception-debug-window)
rlm@206 189
rlm@206 190 controls
rlm@206 191 (merge standard-debug-controls
rlm@206 192 {"key-o"
rlm@206 193 (fn [_ _] (.setEnabled finger-control true))
rlm@206 194 "key-p"
rlm@206 195 (fn [_ _] (.setEnabled finger-control false))
rlm@206 196 "key-k"
rlm@206 197 (fn [_ _] (.setEnabled hand-control true))
rlm@206 198 "key-l"
rlm@206 199 (fn [_ _] (.setEnabled hand-control false))
rlm@206 200 "key-i"
rlm@206 201 (fn [world _] (set-gravity world (Vector3f. 0 0 0)))
rlm@206 202 "key-period"
rlm@206 203 (fn [world _]
rlm@206 204 (.setEnabled finger-control false)
rlm@206 205 (.setEnabled hand-control false)
rlm@206 206 (.rotate creature (doto (Quaternion.)
rlm@206 207 (.fromAngleAxis
rlm@206 208 (float (/ Math/PI 15))
rlm@206 209 (Vector3f. 0 0 -1))))
rlm@206 210
rlm@206 211 (.setEnabled finger-control true)
rlm@206 212 (.setEnabled hand-control true)
rlm@206 213 (set-gravity world (Vector3f. 0 0 0))
rlm@206 214 )
rlm@206 215
rlm@206 216
rlm@206 217 }
rlm@206 218 )
rlm@206 219
rlm@206 220 ]
rlm@206 221 (comment
rlm@206 222 (.setCollisionGroup
rlm@206 223 (.getControl hand RigidBodyControl)
rlm@206 224 PhysicsCollisionObject/COLLISION_GROUP_NONE)
rlm@206 225 )
rlm@206 226 (apply
rlm@206 227 world
rlm@206 228 (with-movement
rlm@206 229 hand
rlm@206 230 ["key-y" "key-u" "key-h" "key-j" "key-n" "key-m"]
rlm@206 231 [10 10 10 10 1 1]
rlm@206 232 (with-movement
rlm@206 233 finger
rlm@206 234 ["key-r" "key-t" "key-f" "key-g" "key-v" "key-b"]
rlm@206 235 [1 1 10 10 10 10]
rlm@206 236 [root
rlm@206 237 controls
rlm@206 238 (fn [world]
rlm@206 239 (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
rlm@206 240 (set-gravity world (Vector3f. 0 0 0))
rlm@206 241 (light-up-everything world))
rlm@206 242 (fn [_ _] (prop-view (list (prop))))]))))))
rlm@206 243
rlm@206 244 #+end_src
rlm@206 245
rlm@206 246
rlm@157 247 * COMMENT generate source
rlm@157 248 #+begin_src clojure :tangle ../src/cortex/proprioception.clj
rlm@157 249 <<proprioception>>
rlm@157 250 #+end_src