Mercurial > cortex

annotate org/proprioception.org @ 180:f1b078375484

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
refactored movement
author Robert McIntyre <rlm@mit.edu>
date Sat, 04 Feb 2012 07:20:12 -0700
parents 0b9ae09eaec3
children 2902aca33c6e
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@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 (defn right-handed?
rlm@173 22 "true iff the three vectors form a right handed coordinate
rlm@173 23 system. The three vectors do not have to be normalized or
rlm@173 24 orthogonal."
rlm@173 25 [vec1 vec2 vec3]
rlm@157 26 (< 0 (.dot (.cross vec1 vec2) vec3)))
rlm@157 27
rlm@173 28 (defn absolute-angle
rlm@173 29 "The angle between 'vec1 and 'vec2. Positive if the angle to get
rlm@173 30 from 'vec1 to 'vec2 is counterclockwise around 'axis, and negative
rlm@173 31 otherwise."
rlm@173 32 [vec1 vec2 axis]
rlm@157 33 (let [angle (.angleBetween vec1 vec2)]
rlm@157 34 (if (right-handed? vec1 vec2 axis)
rlm@157 35 angle (- (* 2 Math/PI) angle))))
rlm@157 36
rlm@173 37 (defn proprioception-fn
rlm@173 38 "Returns a function which returns proprioceptive sensory data when
rlm@173 39 called inside a running simulation."
rlm@173 40 [#^Node parts #^Node joint]
rlm@157 41 (let [[obj-a obj-b] (joint-targets parts joint)
rlm@157 42 joint-rot (.getWorldRotation joint)
rlm@157 43 x0 (.mult joint-rot Vector3f/UNIT_X)
rlm@157 44 y0 (.mult joint-rot Vector3f/UNIT_Y)
rlm@157 45 z0 (.mult joint-rot Vector3f/UNIT_Z)]
rlm@157 46 (println-repl "x:" x0)
rlm@157 47 (println-repl "y:" y0)
rlm@157 48 (println-repl "z:" z0)
rlm@157 49 (println-repl "init-a:" (.getWorldRotation obj-a))
rlm@157 50 (println-repl "init-b:" (.getWorldRotation obj-b))
rlm@157 51
rlm@157 52 (fn []
rlm@157 53 (let [rot-a (.clone (.getWorldRotation obj-a))
rlm@157 54 rot-b (.clone (.getWorldRotation obj-b))
rlm@157 55 x (.mult rot-a x0)
rlm@157 56 y (.mult rot-a y0)
rlm@157 57 z (.mult rot-a z0)
rlm@157 58
rlm@157 59 X (.mult rot-b x0)
rlm@157 60 Y (.mult rot-b y0)
rlm@157 61 Z (.mult rot-b z0)
rlm@157 62 heading (Math/atan2 (.dot X z) (.dot X x))
rlm@157 63 pitch (Math/atan2 (.dot X y) (.dot X x))
rlm@157 64
rlm@157 65 ;; rotate x-vector back to origin
rlm@157 66 reverse
rlm@157 67 (doto (Quaternion.)
rlm@157 68 (.fromAngleAxis
rlm@157 69 (.angleBetween X x)
rlm@157 70 (let [cross (.normalize (.cross X x))]
rlm@157 71 (if (= 0 (.length cross)) y cross))))
rlm@157 72 roll (absolute-angle (.mult reverse Y) y x)]
rlm@157 73 [heading pitch roll]))))
rlm@157 74
rlm@173 75 (defn proprioception!
rlm@173 76 "Endow the creature with the sense of proprioception. Returns a
rlm@173 77 sequence of functions, one for each child of the \"joints\" node in
rlm@173 78 the creature, which each report proprioceptive information about
rlm@173 79 that joint."
rlm@157 80 [#^Node creature]
rlm@157 81 ;; extract the body's joints
rlm@173 82 (let [senses (map (partial proprioception-fn creature)
rlm@173 83 (joints creature))]
rlm@157 84 (fn []
rlm@157 85 (map #(%) senses))))
rlm@175 86
rlm@175 87
rlm@175 88 (import java.awt.image.BufferedImage)
rlm@175 89
rlm@175 90 (defn draw-sprite [image sprite x y color ]
rlm@175 91 (dorun
rlm@175 92 (for [[u v] sprite]
rlm@175 93 (.setRGB image (+ u x) (+ v y) color))))
rlm@175 94
rlm@175 95 (defn view-angle
rlm@175 96 "create a debug view of an angle"
rlm@175 97 [color]
rlm@175 98 (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
rlm@175 99 previous (atom [25 25])
rlm@175 100 sprite [[0 0] [0 1]
rlm@175 101 [0 -1] [-1 0] [1 0]]]
rlm@175 102 (fn [angle]
rlm@175 103 (let [angle (float angle)]
rlm@175 104 (let [position
rlm@175 105 [(+ 25 (int (* 20 (Math/cos angle))))
rlm@175 106 (+ 25 (int (* -20 (Math/sin angle))))]]
rlm@175 107 (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)
rlm@175 108 (draw-sprite image sprite (position 0) (position 1) color)
rlm@175 109 (reset! previous position))
rlm@175 110 image))))
rlm@175 111
rlm@175 112 (defn proprioception-debug-window
rlm@175 113 []
rlm@175 114 (let [heading (view-angle 0xFF0000)
rlm@175 115 pitch (view-angle 0x00FF00)
rlm@175 116 roll (view-angle 0xFFFFFF)
rlm@175 117 v-heading (view-image)
rlm@175 118 v-pitch (view-image)
rlm@175 119 v-roll (view-image)
rlm@175 120 ]
rlm@175 121 (fn [prop-data]
rlm@175 122 (dorun
rlm@175 123 (map
rlm@175 124 (fn [[h p r]]
rlm@175 125 (v-heading (heading h))
rlm@175 126 (v-pitch (pitch p))
rlm@175 127 (v-roll (roll r)))
rlm@175 128 prop-data)))))
rlm@175 129
rlm@157 130 #+end_src
rlm@157 131
rlm@157 132 * COMMENT generate source
rlm@157 133 #+begin_src clojure :tangle ../src/cortex/proprioception.clj
rlm@157 134 <<proprioception>>
rlm@157 135 #+end_src