annotate org/proprioception.org @ 157:84c67be00abe

refactored proprioception
author Robert McIntyre <rlm@mit.edu>
date Fri, 03 Feb 2012 06:21:39 -0700
parents
children 1943b3f581c2
rev   line source
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