|
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@157
|
12 (:use (cortex world util sense body)))
|
|
rlm@157
|
13
|
|
rlm@157
|
14 (cortex.import/mega-import-jme3)
|
|
rlm@157
|
15
|
|
rlm@157
|
16 (defn right-handed? [vec1 vec2 vec3]
|
|
rlm@157
|
17 (< 0 (.dot (.cross vec1 vec2) vec3)))
|
|
rlm@157
|
18
|
|
rlm@157
|
19 (defn absolute-angle [vec1 vec2 axis]
|
|
rlm@157
|
20 (let [angle (.angleBetween vec1 vec2)]
|
|
rlm@157
|
21 (if (right-handed? vec1 vec2 axis)
|
|
rlm@157
|
22 angle (- (* 2 Math/PI) angle))))
|
|
rlm@157
|
23
|
|
rlm@157
|
24
|
|
rlm@157
|
25 (defn joint-proprioception [#^Node parts #^Node joint]
|
|
rlm@157
|
26 (let [[obj-a obj-b] (joint-targets parts joint)
|
|
rlm@157
|
27 joint-rot (.getWorldRotation joint)
|
|
rlm@157
|
28 x0 (.mult joint-rot Vector3f/UNIT_X)
|
|
rlm@157
|
29 y0 (.mult joint-rot Vector3f/UNIT_Y)
|
|
rlm@157
|
30 z0 (.mult joint-rot Vector3f/UNIT_Z)]
|
|
rlm@157
|
31 (println-repl "x:" x0)
|
|
rlm@157
|
32 (println-repl "y:" y0)
|
|
rlm@157
|
33 (println-repl "z:" z0)
|
|
rlm@157
|
34 (println-repl "init-a:" (.getWorldRotation obj-a))
|
|
rlm@157
|
35 (println-repl "init-b:" (.getWorldRotation obj-b))
|
|
rlm@157
|
36
|
|
rlm@157
|
37 (fn []
|
|
rlm@157
|
38 (let [rot-a (.clone (.getWorldRotation obj-a))
|
|
rlm@157
|
39 rot-b (.clone (.getWorldRotation obj-b))
|
|
rlm@157
|
40 x (.mult rot-a x0)
|
|
rlm@157
|
41 y (.mult rot-a y0)
|
|
rlm@157
|
42 z (.mult rot-a z0)
|
|
rlm@157
|
43
|
|
rlm@157
|
44 X (.mult rot-b x0)
|
|
rlm@157
|
45 Y (.mult rot-b y0)
|
|
rlm@157
|
46 Z (.mult rot-b z0)
|
|
rlm@157
|
47 heading (Math/atan2 (.dot X z) (.dot X x))
|
|
rlm@157
|
48 pitch (Math/atan2 (.dot X y) (.dot X x))
|
|
rlm@157
|
49
|
|
rlm@157
|
50 ;; rotate x-vector back to origin
|
|
rlm@157
|
51 reverse
|
|
rlm@157
|
52 (doto (Quaternion.)
|
|
rlm@157
|
53 (.fromAngleAxis
|
|
rlm@157
|
54 (.angleBetween X x)
|
|
rlm@157
|
55 (let [cross (.normalize (.cross X x))]
|
|
rlm@157
|
56 (if (= 0 (.length cross)) y cross))))
|
|
rlm@157
|
57 roll (absolute-angle (.mult reverse Y) y x)]
|
|
rlm@157
|
58
|
|
rlm@157
|
59 [heading pitch roll]))))
|
|
rlm@157
|
60
|
|
rlm@157
|
61 (defn proprioception
|
|
rlm@157
|
62 "Create a function that provides proprioceptive information about an
|
|
rlm@157
|
63 entire body."
|
|
rlm@157
|
64 [#^Node creature]
|
|
rlm@157
|
65 ;; extract the body's joints
|
|
rlm@157
|
66 (let [joints (creature-joints creature)
|
|
rlm@157
|
67 senses (map (partial joint-proprioception creature) joints)]
|
|
rlm@157
|
68 (fn []
|
|
rlm@157
|
69 (map #(%) senses))))
|
|
rlm@157
|
70
|
|
rlm@157
|
71
|
|
rlm@157
|
72
|
|
rlm@157
|
73
|
|
rlm@157
|
74
|
|
rlm@157
|
75 #+end_src
|
|
rlm@157
|
76
|
|
rlm@157
|
77
|
|
rlm@157
|
78
|
|
rlm@157
|
79
|
|
rlm@157
|
80
|
|
rlm@157
|
81
|
|
rlm@157
|
82
|
|
rlm@157
|
83
|
|
rlm@157
|
84 * COMMENT generate source
|
|
rlm@157
|
85 #+begin_src clojure :tangle ../src/cortex/proprioception.clj
|
|
rlm@157
|
86 <<proprioception>>
|
|
rlm@157
|
87 #+end_src
|
