|
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@173
|
17 (:use clojure.contrib.def))
|
|
rlm@173
|
18
|
|
rlm@157
|
19
|
|
rlm@157
|
20 (cortex.import/mega-import-jme3)
|
|
rlm@157
|
21
|
|
rlm@173
|
22 (defvar
|
|
rlm@173
|
23 ^{:arglists '([creature])}
|
|
rlm@173
|
24 joints
|
|
rlm@173
|
25 (sense-nodes "joints")
|
|
rlm@173
|
26 "Return the children of the creature's \"joints\" node.")
|
|
rlm@173
|
27
|
|
rlm@173
|
28 (defn right-handed?
|
|
rlm@173
|
29 "true iff the three vectors form a right handed coordinate
|
|
rlm@173
|
30 system. The three vectors do not have to be normalized or
|
|
rlm@173
|
31 orthogonal."
|
|
rlm@173
|
32 [vec1 vec2 vec3]
|
|
rlm@157
|
33 (< 0 (.dot (.cross vec1 vec2) vec3)))
|
|
rlm@157
|
34
|
|
rlm@173
|
35 (defn absolute-angle
|
|
rlm@173
|
36 "The angle between 'vec1 and 'vec2. Positive if the angle to get
|
|
rlm@173
|
37 from 'vec1 to 'vec2 is counterclockwise around 'axis, and negative
|
|
rlm@173
|
38 otherwise."
|
|
rlm@173
|
39 [vec1 vec2 axis]
|
|
rlm@157
|
40 (let [angle (.angleBetween vec1 vec2)]
|
|
rlm@157
|
41 (if (right-handed? vec1 vec2 axis)
|
|
rlm@157
|
42 angle (- (* 2 Math/PI) angle))))
|
|
rlm@157
|
43
|
|
rlm@173
|
44 (defn proprioception-fn
|
|
rlm@173
|
45 "Returns a function which returns proprioceptive sensory data when
|
|
rlm@173
|
46 called inside a running simulation."
|
|
rlm@173
|
47 [#^Node parts #^Node joint]
|
|
rlm@157
|
48 (let [[obj-a obj-b] (joint-targets parts joint)
|
|
rlm@157
|
49 joint-rot (.getWorldRotation joint)
|
|
rlm@157
|
50 x0 (.mult joint-rot Vector3f/UNIT_X)
|
|
rlm@157
|
51 y0 (.mult joint-rot Vector3f/UNIT_Y)
|
|
rlm@157
|
52 z0 (.mult joint-rot Vector3f/UNIT_Z)]
|
|
rlm@157
|
53 (println-repl "x:" x0)
|
|
rlm@157
|
54 (println-repl "y:" y0)
|
|
rlm@157
|
55 (println-repl "z:" z0)
|
|
rlm@157
|
56 (println-repl "init-a:" (.getWorldRotation obj-a))
|
|
rlm@157
|
57 (println-repl "init-b:" (.getWorldRotation obj-b))
|
|
rlm@157
|
58
|
|
rlm@157
|
59 (fn []
|
|
rlm@157
|
60 (let [rot-a (.clone (.getWorldRotation obj-a))
|
|
rlm@157
|
61 rot-b (.clone (.getWorldRotation obj-b))
|
|
rlm@157
|
62 x (.mult rot-a x0)
|
|
rlm@157
|
63 y (.mult rot-a y0)
|
|
rlm@157
|
64 z (.mult rot-a z0)
|
|
rlm@157
|
65
|
|
rlm@157
|
66 X (.mult rot-b x0)
|
|
rlm@157
|
67 Y (.mult rot-b y0)
|
|
rlm@157
|
68 Z (.mult rot-b z0)
|
|
rlm@157
|
69 heading (Math/atan2 (.dot X z) (.dot X x))
|
|
rlm@157
|
70 pitch (Math/atan2 (.dot X y) (.dot X x))
|
|
rlm@157
|
71
|
|
rlm@157
|
72 ;; rotate x-vector back to origin
|
|
rlm@157
|
73 reverse
|
|
rlm@157
|
74 (doto (Quaternion.)
|
|
rlm@157
|
75 (.fromAngleAxis
|
|
rlm@157
|
76 (.angleBetween X x)
|
|
rlm@157
|
77 (let [cross (.normalize (.cross X x))]
|
|
rlm@157
|
78 (if (= 0 (.length cross)) y cross))))
|
|
rlm@157
|
79 roll (absolute-angle (.mult reverse Y) y x)]
|
|
rlm@157
|
80 [heading pitch roll]))))
|
|
rlm@157
|
81
|
|
rlm@173
|
82 (defn proprioception!
|
|
rlm@173
|
83 "Endow the creature with the sense of proprioception. Returns a
|
|
rlm@173
|
84 sequence of functions, one for each child of the \"joints\" node in
|
|
rlm@173
|
85 the creature, which each report proprioceptive information about
|
|
rlm@173
|
86 that joint."
|
|
rlm@157
|
87 [#^Node creature]
|
|
rlm@157
|
88 ;; extract the body's joints
|
|
rlm@173
|
89 (let [senses (map (partial proprioception-fn creature)
|
|
rlm@173
|
90 (joints creature))]
|
|
rlm@157
|
91 (fn []
|
|
rlm@157
|
92 (map #(%) senses))))
|
|
rlm@157
|
93
|
|
rlm@157
|
94
|
|
rlm@157
|
95
|
|
rlm@157
|
96
|
|
rlm@157
|
97
|
|
rlm@157
|
98 #+end_src
|
|
rlm@157
|
99
|
|
rlm@157
|
100
|
|
rlm@157
|
101
|
|
rlm@157
|
102
|
|
rlm@157
|
103
|
|
rlm@157
|
104
|
|
rlm@157
|
105
|
|
rlm@157
|
106
|
|
rlm@157
|
107 * COMMENT generate source
|
|
rlm@157
|
108 #+begin_src clojure :tangle ../src/cortex/proprioception.clj
|
|
rlm@157
|
109 <<proprioception>>
|
|
rlm@157
|
110 #+end_src
|
