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