view org/proprioception.org @ 190:2902aca33c6e

added proprioception debug view.
author Robert McIntyre <rlm@mit.edu>
date Sat, 04 Feb 2012 10:26:11 -0700
parents 0b9ae09eaec3
children df46a609fed9
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 * COMMENT generate source
152 #+begin_src clojure :tangle ../src/cortex/proprioception.clj
153 <<proprioception>>
154 #+end_src