view org/proprioception.org @ 240:6961377c4554

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