Mercurial > cortex

comparison org/proprioception.org @ 157:84c67be00abe

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