Mercurial > cortex

annotate org/proprioception.org @ 219:5f14fd7b1288

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
minor corrections from reviewing with dad
author Robert McIntyre <rlm@mit.edu>
date Sat, 11 Feb 2012 00:51:54 -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