rlm@157: #+title: The Sense of Proprioception rlm@157: #+author: Robert McIntyre rlm@157: #+email: rlm@mit.edu rlm@157: #+description: proprioception for simulated creatures rlm@157: #+keywords: simulation, jMonkeyEngine3, clojure rlm@157: #+SETUPFILE: ../../aurellem/org/setup.org rlm@157: #+INCLUDE: ../../aurellem/org/level-0.org rlm@157: rlm@157: #+name: proprioception rlm@157: #+begin_src clojure rlm@157: (ns cortex.proprioception rlm@173: "Simulate the sense of proprioception (ability to detect the rlm@173: relative positions of body parts with repsect to other body parts) rlm@173: in jMonkeyEngine3. Reads specially prepared blender files to rlm@173: automatically generate proprioceptive senses." rlm@173: (:use (cortex world util sense body)) rlm@174: (:use clojure.contrib.def) rlm@174: (:import com.jme3.scene.Node) rlm@190: (:import java.awt.image.BufferedImage) rlm@174: (:import (com.jme3.math Vector3f Quaternion))) rlm@157: rlm@173: (defn right-handed? rlm@173: "true iff the three vectors form a right handed coordinate rlm@173: system. The three vectors do not have to be normalized or rlm@173: orthogonal." rlm@173: [vec1 vec2 vec3] rlm@157: (< 0 (.dot (.cross vec1 vec2) vec3))) rlm@157: rlm@173: (defn absolute-angle rlm@173: "The angle between 'vec1 and 'vec2. Positive if the angle to get rlm@173: from 'vec1 to 'vec2 is counterclockwise around 'axis, and negative rlm@173: otherwise." rlm@173: [vec1 vec2 axis] rlm@157: (let [angle (.angleBetween vec1 vec2)] rlm@157: (if (right-handed? vec1 vec2 axis) rlm@157: angle (- (* 2 Math/PI) angle)))) rlm@157: rlm@173: (defn proprioception-fn rlm@173: "Returns a function which returns proprioceptive sensory data when rlm@173: called inside a running simulation." rlm@173: [#^Node parts #^Node joint] rlm@157: (let [[obj-a obj-b] (joint-targets parts joint) rlm@157: joint-rot (.getWorldRotation joint) rlm@157: x0 (.mult joint-rot Vector3f/UNIT_X) rlm@157: y0 (.mult joint-rot Vector3f/UNIT_Y) rlm@157: z0 (.mult joint-rot Vector3f/UNIT_Z)] rlm@157: (println-repl "x:" x0) rlm@157: (println-repl "y:" y0) rlm@157: (println-repl "z:" z0) rlm@157: (println-repl "init-a:" (.getWorldRotation obj-a)) rlm@157: (println-repl "init-b:" (.getWorldRotation obj-b)) rlm@157: rlm@157: (fn [] rlm@157: (let [rot-a (.clone (.getWorldRotation obj-a)) rlm@157: rot-b (.clone (.getWorldRotation obj-b)) rlm@157: x (.mult rot-a x0) rlm@157: y (.mult rot-a y0) rlm@157: z (.mult rot-a z0) rlm@157: rlm@157: X (.mult rot-b x0) rlm@157: Y (.mult rot-b y0) rlm@157: Z (.mult rot-b z0) rlm@157: heading (Math/atan2 (.dot X z) (.dot X x)) rlm@157: pitch (Math/atan2 (.dot X y) (.dot X x)) rlm@157: rlm@157: ;; rotate x-vector back to origin rlm@157: reverse rlm@157: (doto (Quaternion.) rlm@157: (.fromAngleAxis rlm@157: (.angleBetween X x) rlm@157: (let [cross (.normalize (.cross X x))] rlm@157: (if (= 0 (.length cross)) y cross)))) rlm@157: roll (absolute-angle (.mult reverse Y) y x)] rlm@157: [heading pitch roll])))) rlm@157: rlm@173: (defn proprioception! rlm@173: "Endow the creature with the sense of proprioception. Returns a rlm@173: sequence of functions, one for each child of the \"joints\" node in rlm@173: the creature, which each report proprioceptive information about rlm@173: that joint." rlm@157: [#^Node creature] rlm@157: ;; extract the body's joints rlm@173: (let [senses (map (partial proprioception-fn creature) rlm@173: (joints creature))] rlm@157: (fn [] rlm@157: (map #(%) senses)))) rlm@175: rlm@175: rlm@175: (defn draw-sprite [image sprite x y color ] rlm@175: (dorun rlm@175: (for [[u v] sprite] rlm@175: (.setRGB image (+ u x) (+ v y) color)))) rlm@175: rlm@190: rlm@175: (defn view-angle rlm@175: "create a debug view of an angle" rlm@175: [color] rlm@175: (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB) rlm@175: previous (atom [25 25]) rlm@175: sprite [[0 0] [0 1] rlm@175: [0 -1] [-1 0] [1 0]]] rlm@175: (fn [angle] rlm@175: (let [angle (float angle)] rlm@175: (let [position rlm@175: [(+ 25 (int (* 20 (Math/cos angle)))) rlm@175: (+ 25 (int (* -20 (Math/sin angle))))]] rlm@175: (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000) rlm@175: (draw-sprite image sprite (position 0) (position 1) color) rlm@175: (reset! previous position)) rlm@175: image)))) rlm@175: rlm@190: rlm@190: (defn proprioception-display-kernel rlm@190: "Display proprioception angles in a BufferedImage" rlm@190: [[h p r]] rlm@190: (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB) rlm@190: previous-heading (atom [25 25]) rlm@190: previous-pitch (atom [25 25]) rlm@190: previous-roll (atom [25 25]) rlm@190: rlm@190: heading-sprite [[0 0] [0 1] [0 -1] [-1 0] [1 0]] rlm@190: pitch-sprite [[0 0] [0 1] [0 -1] [-1 0] [1 0]] rlm@190: roll-sprite [[0 0] [0 1] [0 -1] [-1 0] [1 0]] rlm@190: draw-angle rlm@190: (fn [angle sprite previous color] rlm@190: (let [angle (float angle)] rlm@190: (let [position rlm@190: [(+ 25 (int (* 20 (Math/cos angle)))) rlm@190: (+ 25 (int (* -20 (Math/sin angle))))]] rlm@190: (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000) rlm@190: (draw-sprite image sprite (position 0) (position 1) color) rlm@190: (reset! previous position)) rlm@190: image))] rlm@190: (dorun (map draw-angle rlm@190: [h p r] rlm@190: [heading-sprite pitch-sprite roll-sprite] rlm@190: [previous-heading previous-pitch previous-roll] rlm@190: [0xFF0000 0x00FF00 0xFFFFFF])) rlm@190: image)) rlm@190: rlm@190: (defn view-proprioception rlm@190: "Creates a function which accepts a list of proprioceptive data and rlm@190: display each element of the list to the screen as an image." rlm@175: [] rlm@190: (view-sense proprioception-display-kernel)) rlm@190: rlm@190: rlm@175: rlm@157: #+end_src rlm@157: rlm@157: * COMMENT generate source rlm@157: #+begin_src clojure :tangle ../src/cortex/proprioception.clj rlm@157: <> rlm@157: #+end_src