rlm@37: #+title: Simulated Sense of Touch
rlm@0: #+author: Robert McIntyre
rlm@0: #+email: rlm@mit.edu
rlm@37: #+description: Simulated touch for AI research using JMonkeyEngine and clojure.
rlm@37: #+keywords: simulation, tactile sense, jMonkeyEngine3, clojure
rlm@4: #+SETUPFILE: ../../aurellem/org/setup.org
rlm@4: #+INCLUDE: ../../aurellem/org/level-0.org
rlm@0: 
rlm@37: * Touch
rlm@0: 
rlm@226: Touch is critical to navigation and spatial reasoning and as such I
rlm@226: need a simulated version of it to give to my AI creatures.
rlm@0: 
rlm@228: However, touch in my virtual can not exactly correspond to human touch
rlm@228: because my creatures are made out of completely rigid segments that
rlm@228: don't deform like human skin.
rlm@228: 
rlm@228: Human skin has a wide array of touch sensors, each of which speciliaze
rlm@228: in detecting different vibrational modes and pressures. These sensors
rlm@228: can integrate a vast expanse of skin (i.e. your entire palm), or a
rlm@228: tiny patch of skin at the tip of your finger. The hairs of the skin
rlm@228: help detect objects before they even come into contact with the skin
rlm@228: proper. 
rlm@228: 
rlm@228: Instead of measuring deformation or vibration, I surround each rigid
rlm@228: part with a plenitude of hair-like objects which do not interact with
rlm@228: the physical world. Physical objects can pass through them with no
rlm@228: effect. The hairs are able to measure contact with other objects, and
rlm@228: constantly report how much of their extent is covered. So, even though
rlm@228: the creature's body parts do not deform, the hairs create a margin
rlm@228: around those body parts which achieves a sense of touch which is a
rlm@228: hybrid between a human's sense of deformation and sense from hairs.
rlm@228: 
rlm@228: Implementing touch in jMonkeyEngine follows a different techinal route
rlm@228: than vision and hearing. Those two senses piggybacked off
rlm@228: jMonkeyEngine's 3D audio and video rendering subsystems. To simulate
rlm@228: Touch, I use jMonkeyEngine's physics system to execute many small
rlm@229: collision detections, one for each "hair". The placement of the
rlm@229: "hairs" is determined by a UV-mapped image which shows where each hair
rlm@229: should be on the 3D surface of the body.
rlm@228: 
rlm@229: * Defining Touch Meta-Data in Blender
rlm@229: 
rlm@245: Each geometry can have a single UV map which describes the position of
rlm@245: the "hairs" which will constitute its sense of touch. This image path
rlm@245: is stored under the "touch" key.  The image itself is black and white,
rlm@245: with black meaning a hair length of 0 (no hair is present) and white
rlm@245: meaning a hair length of =scale=, which is a float stored under the
rlm@245: key "scale". I call these "hairs" /feelers/.
rlm@229: 
rlm@231: #+name: meta-data
rlm@0: #+begin_src clojure
rlm@229: (defn tactile-sensor-profile
rlm@229:   "Return the touch-sensor distribution image in BufferedImage format,
rlm@229:    or nil if it does not exist."
rlm@229:   [#^Geometry obj]
rlm@229:   (if-let [image-path (meta-data obj "touch")]
rlm@229:     (load-image image-path)))
rlm@233: 
rlm@233: (defn tactile-scale
rlm@233:   "Return the maximum length of a hair.  All hairs are scalled between
rlm@233:    0.0 and this length, depending on their color. Black is 0, and
rlm@233:    white is maximum length, and everything in between is scalled
rlm@233:    linearlly. Default scale is 0.01 jMonkeyEngine units."
rlm@233:   [#^Geometry obj]
rlm@233:   (if-let [scale (meta-data obj "scale")]
rlm@233:     scale 0.1))
rlm@228: #+end_src
rlm@156: 
rlm@246: Here is an example of a UV-map which specifies the position of touch
rlm@246: sensors along the surface of the worm.
rlm@229: 
rlm@246: #+attr_html: width=755
rlm@246: #+caption: This is the tactile-sensor-profile for the upper segment of the worm. It defines regions of high touch sensitivity (where there are many white pixels) and regions of low sensitivity (where white pixels are sparse).
rlm@246: [[../images/finger-UV.png]]
rlm@234: 
rlm@233: * Skin Creation
rlm@238: 
rlm@238: =(touch-kernel)= generates the functions which implement the sense of
rlm@238: touch for a creature. These functions must do 6 things to obtain touch
rlm@238: data.
rlm@238: 
rlm@238:   - Get the tactile profile image and scale paramaters which describe
rlm@238:     the layout of feelers along the object's surface.
rlm@239:     =(tactile-sensor-profile)=, =(tactile-scale)=
rlm@239: 
rlm@238:   - Find the triangles which make up the mesh in pixel-space and in
rlm@238:     world-space.
rlm@239:     =(triangles)= =(pixel-triangles)=
rlm@239:     
rlm@239:   - Find the coordinates of each pixel in pixel space. These
rlm@239:     coordinates are used to make the touch-topology.
rlm@240:     =(feeler-pixel-coords)=
rlm@239: 
rlm@238:   - Find the coordinates of each pixel in world-space. These
rlm@240:     coordinates are the origins of the feelers. =(feeler-origins)=
rlm@239:     
rlm@238:   - Calculate the normals of the triangles in world space, and add
rlm@238:     them to each of the origins of the feelers. These are the
rlm@238:     normalized coordinates of the tips of the feelers.
rlm@240:     For both of these, =(feeler-tips)=
rlm@239: 
rlm@238:   - Generate some sort of topology for the sensors.
rlm@239:     =(touch-topology)=
rlm@239: 
rlm@238: 
rlm@233: #+name: kernel
rlm@233: #+begin_src clojure
rlm@233: (in-ns 'cortex.touch)
rlm@233: 
rlm@244: (defn set-ray [#^Ray ray #^Matrix4f transform
rlm@244:                #^Vector3f origin #^Vector3f tip]
rlm@243:   ;; Doing everything locally recduces garbage collection by enough to
rlm@243:   ;; be worth it.
rlm@243:   (.mult transform origin (.getOrigin ray))
rlm@243:   
rlm@243:   (.mult transform tip (.getDirection ray))
rlm@244:   (.subtractLocal (.getDirection ray) (.getOrigin ray)))
rlm@242: 
rlm@233: (defn touch-kernel
rlm@234:   "Constructs a function which will return tactile sensory data from
rlm@234:    'geo when called from inside a running simulation"
rlm@234:   [#^Geometry geo]
rlm@243:   (if-let
rlm@243:       [profile (tactile-sensor-profile geo)]
rlm@243:     (let [ray-reference-origins (feeler-origins geo profile)
rlm@243:           ray-reference-tips (feeler-tips geo profile)
rlm@244:           ray-length (tactile-scale geo)
rlm@243:           current-rays (map (fn [_] (Ray.)) ray-reference-origins)
rlm@243:           topology (touch-topology geo profile)]
rlm@244:       (dorun (map #(.setLimit % ray-length) current-rays))
rlm@233:       (fn [node]
rlm@243:         (let [transform (.getWorldMatrix geo)]
rlm@243:           (dorun
rlm@244:            (map (fn [ray ref-origin ref-tip]
rlm@244:                   (set-ray ray transform ref-origin ref-tip))
rlm@243:                 current-rays ray-reference-origins
rlm@244:                 ray-reference-tips))
rlm@233:           (vector
rlm@243:            topology
rlm@233:            (vec
rlm@243:             (for [ray current-rays]
rlm@233:               (do
rlm@233:                 (let [results (CollisionResults.)]
rlm@233:                   (.collideWith node ray results)
rlm@233:                   (let [touch-objects
rlm@233:                         (filter #(not (= geo (.getGeometry %)))
rlm@233:                                 results)]
rlm@233:                     [(if (empty? touch-objects)
rlm@243:                        (.getLimit ray)
rlm@243:                        (.getDistance (first touch-objects)))
rlm@243:                      (.getLimit ray)])))))))))))
rlm@233: 
rlm@233: (defn touch! 
rlm@233:   "Endow the creature with the sense of touch. Returns a sequence of
rlm@233:    functions, one for each body part with a tactile-sensor-proile,
rlm@233:    each of which when called returns sensory data for that body part."
rlm@233:   [#^Node creature]
rlm@233:   (filter
rlm@233:    (comp not nil?)
rlm@233:    (map touch-kernel
rlm@233:         (filter #(isa? (class %) Geometry)
rlm@233:                 (node-seq creature)))))
rlm@233: #+end_src
rlm@233: 
rlm@238: * Sensor Related Functions
rlm@238: 
rlm@238: These functions analyze the touch-sensor-profile image convert the
rlm@238: location of each touch sensor from pixel coordinates to UV-coordinates
rlm@238: and XYZ-coordinates.
rlm@238: 
rlm@238: #+name: sensors
rlm@238: #+begin_src clojure
rlm@240: (in-ns 'cortex.touch)
rlm@240: 
rlm@240: (defn feeler-pixel-coords
rlm@239:  "Returns the coordinates of the feelers in pixel space in lists, one
rlm@239:   list for each triangle, ordered in the same way as (triangles) and
rlm@239:   (pixel-triangles)."
rlm@239:  [#^Geometry geo image]
rlm@240:  (map 
rlm@240:   (fn [pixel-triangle]
rlm@240:     (filter
rlm@240:      (fn [coord]
rlm@240:        (inside-triangle? (->triangle pixel-triangle)
rlm@240:                          (->vector3f coord)))
rlm@240:        (white-coordinates image (convex-bounds pixel-triangle))))
rlm@240:   (pixel-triangles geo image)))
rlm@239: 
rlm@242: (defn feeler-world-coords [#^Geometry geo image]
rlm@240:   (let [transforms
rlm@240:         (map #(triangles->affine-transform
rlm@240:                (->triangle %1) (->triangle %2))
rlm@240:              (pixel-triangles geo image)
rlm@240:              (triangles geo))]
rlm@242:     (map (fn [transform coords]
rlm@240:               (map #(.mult transform (->vector3f %)) coords))
rlm@240:             transforms (feeler-pixel-coords geo image))))
rlm@239: 
rlm@242: (defn feeler-origins [#^Geometry geo image]
rlm@242:    (reduce concat (feeler-world-coords geo image)))
rlm@242: 
rlm@240: (defn feeler-tips [#^Geometry geo image]
rlm@242:   (let [world-coords (feeler-world-coords geo image)
rlm@241:         normals
rlm@241:         (map
rlm@241:          (fn [triangle]
rlm@241:            (.calculateNormal triangle)
rlm@241:            (.clone (.getNormal triangle)))
rlm@241:          (map ->triangle (triangles geo)))]
rlm@242: 
rlm@242:     (mapcat (fn [origins normal]
rlm@242:               (map #(.add % normal) origins))
rlm@242:             world-coords normals)))
rlm@241: 
rlm@241: (defn touch-topology [#^Geometry geo image]
rlm@243:   (collapse (reduce concat (feeler-pixel-coords geo image))))
rlm@238: #+end_src
rlm@238: 
rlm@233: * Visualizing Touch
rlm@233: #+name: visualization
rlm@233: #+begin_src clojure
rlm@233: (in-ns 'cortex.touch)
rlm@233: 
rlm@233: (defn touch->gray
rlm@245:   "Convert a pair of [distance, max-distance] into a grayscale pixel." 
rlm@233:   [distance max-distance]
rlm@245:   (gray (- 255 (rem (int (* 255 (/ distance max-distance))) 256))))
rlm@233: 
rlm@233: (defn view-touch 
rlm@245:   "Creates a function which accepts a list of touch sensor-data and
rlm@233:    displays each element to the screen."
rlm@233:   []
rlm@233:   (view-sense
rlm@246:    (fn [[coords sensor-data]]
rlm@233:      (let [image (points->image coords)]
rlm@233:        (dorun
rlm@233:         (for [i (range (count coords))]
rlm@233:           (.setRGB image ((coords i) 0) ((coords i) 1)
rlm@246:                    (apply touch->gray (sensor-data i))))) image))))
rlm@233: #+end_src
rlm@233: 
rlm@233: 
rlm@233: 
rlm@228: * Triangle Manipulation Functions
rlm@228: 
rlm@229: The rigid bodies which make up a creature have an underlying
rlm@229: =Geometry=, which is a =Mesh= plus a =Material= and other important
rlm@229: data involved with displaying the body. 
rlm@229: 
rlm@229: A =Mesh= is composed of =Triangles=, and each =Triangle= has three
rlm@229: verticies which have coordinates in XYZ space and UV space.
rlm@229:  
rlm@229: Here, =(triangles)= gets all the triangles which compose a mesh, and
rlm@229: =(triangle-UV-coord)= returns the the UV coordinates of the verticies
rlm@229: of a triangle.
rlm@229: 
rlm@231: #+name: triangles-1
rlm@228: #+begin_src clojure
rlm@239: (in-ns 'cortex.touch)
rlm@239: 
rlm@239: (defn vector3f-seq [#^Vector3f v]
rlm@239:   [(.getX v) (.getY v) (.getZ v)])
rlm@239: 
rlm@239: (defn triangle-seq [#^Triangle tri]
rlm@239:   [(vector3f-seq (.get1 tri))
rlm@239:    (vector3f-seq (.get2 tri))
rlm@239:    (vector3f-seq (.get3 tri))])
rlm@239: 
rlm@240: (defn ->vector3f 
rlm@240:   ([coords] (Vector3f. (nth coords 0 0)
rlm@240:                        (nth coords 1 0)
rlm@240:                        (nth coords 2 0))))
rlm@239: 
rlm@239: (defn ->triangle [points]
rlm@239:   (apply #(Triangle. %1 %2 %3) (map ->vector3f points)))
rlm@239: 
rlm@239: (defn triangle
rlm@245:   "Get the triangle specified by triangle-index from the mesh."
rlm@239:   [#^Geometry geo triangle-index]
rlm@239:   (triangle-seq
rlm@239:    (let [scratch (Triangle.)]
rlm@239:      (.getTriangle (.getMesh geo) triangle-index scratch) scratch)))
rlm@239: 
rlm@228: (defn triangles
rlm@228:   "Return a sequence of all the Triangles which compose a given
rlm@228:    Geometry." 
rlm@239:   [#^Geometry geo]
rlm@239:   (map (partial triangle geo) (range (.getTriangleCount (.getMesh geo)))))
rlm@228: 
rlm@228: (defn triangle-vertex-indices
rlm@228:   "Get the triangle vertex indices of a given triangle from a given
rlm@228:    mesh."
rlm@228:   [#^Mesh mesh triangle-index]
rlm@228:   (let [indices (int-array 3)]
rlm@228:     (.getTriangle mesh triangle-index indices)
rlm@228:     (vec indices)))
rlm@228: 
rlm@228: (defn vertex-UV-coord
rlm@228:   "Get the UV-coordinates of the vertex named by vertex-index"
rlm@228:   [#^Mesh mesh vertex-index]
rlm@228:   (let [UV-buffer
rlm@228:         (.getData
rlm@228:          (.getBuffer
rlm@228:           mesh
rlm@228:           VertexBuffer$Type/TexCoord))]
rlm@228:     [(.get UV-buffer (* vertex-index 2))
rlm@228:      (.get UV-buffer (+ 1 (* vertex-index 2)))]))
rlm@228: 
rlm@239: (defn pixel-triangle [#^Geometry geo image index]
rlm@239:   (let [mesh (.getMesh geo)
rlm@239:         width (.getWidth image)
rlm@239:         height (.getHeight image)]
rlm@239:     (vec (map (fn [[u v]] (vector (* width u) (* height v)))
rlm@239:               (map (partial vertex-UV-coord mesh)
rlm@239:                    (triangle-vertex-indices mesh index))))))
rlm@228: 
rlm@239: (defn pixel-triangles [#^Geometry geo image]
rlm@239:  (let [height (.getHeight image)
rlm@239:        width (.getWidth image)]
rlm@239:    (map (partial pixel-triangle geo image)
rlm@239:         (range (.getTriangleCount (.getMesh geo))))))
rlm@229: 
rlm@228: #+end_src
rlm@228: 
rlm@228: * Triangle Affine Transforms
rlm@228: 
rlm@229: The position of each hair is stored in a 2D image in UV
rlm@229: coordinates. To place the hair in 3D space we must convert from UV
rlm@229: coordinates to XYZ coordinates. Each =Triangle= has coordinates in
rlm@229: both UV-space and XYZ-space, which defines a unique [[http://mathworld.wolfram.com/AffineTransformation.html ][Affine Transform]]
rlm@229: for translating any coordinate within the UV triangle to the
rlm@229: cooresponding coordinate in the XYZ triangle.
rlm@229: 
rlm@231: #+name: triangles-3
rlm@228: #+begin_src clojure
rlm@243: (in-ns 'cortex.touch)
rlm@243: 
rlm@228: (defn triangle->matrix4f
rlm@228:   "Converts the triangle into a 4x4 matrix: The first three columns
rlm@228:    contain the vertices of the triangle; the last contains the unit
rlm@228:    normal of the triangle. The bottom row is filled with 1s."
rlm@228:   [#^Triangle t]
rlm@228:   (let [mat (Matrix4f.)
rlm@228:         [vert-1 vert-2 vert-3]
rlm@228:         ((comp vec map) #(.get t %) (range 3))
rlm@228:         unit-normal (do (.calculateNormal t)(.getNormal t))
rlm@228:         vertices [vert-1 vert-2 vert-3 unit-normal]]
rlm@228:     (dorun 
rlm@228:      (for [row (range 4) col (range 3)]
rlm@228:        (do
rlm@228:          (.set mat col row (.get (vertices row)col))
rlm@245:          (.set mat 3 row 1)))) mat))
rlm@228: 
rlm@240: (defn triangles->affine-transform
rlm@228:   "Returns the affine transformation that converts each vertex in the
rlm@228:    first triangle into the corresponding vertex in the second
rlm@228:    triangle."
rlm@228:   [#^Triangle tri-1 #^Triangle tri-2]
rlm@228:   (.mult 
rlm@228:    (triangle->matrix4f tri-2)
rlm@228:    (.invert (triangle->matrix4f tri-1))))
rlm@228: #+end_src
rlm@228: 
rlm@239: 
rlm@239: * Schrapnel Conversion Functions
rlm@239: 
rlm@239: It is convienent to treat a =Triangle= as a sequence of verticies, and
rlm@239: a =Vector2f= and =Vector3f= as a sequence of floats. These conversion
rlm@239: functions make this easy. If these classes implemented =Iterable= then
rlm@239: this code would not be necessary. Hopefully they will in the future.
rlm@239: 
rlm@229: * Triangle Boundaries
rlm@229:   
rlm@229: For efficiency's sake I will divide the UV-image into small squares
rlm@229: which inscribe each UV-triangle, then extract the points which lie
rlm@229: inside the triangle and map them to 3D-space using
rlm@229: =(triangle-transform)= above. To do this I need a function,
rlm@229: =(inside-triangle?)=, which determines whether a point is inside a
rlm@229: triangle in 2D UV-space.
rlm@228: 
rlm@231: #+name: triangles-4
rlm@228: #+begin_src clojure
rlm@229: (defn convex-bounds
rlm@229:   "Returns the smallest square containing the given vertices, as a
rlm@229:    vector of integers [left top width height]."
rlm@240:   [verts]
rlm@240:   (let [xs (map first verts)
rlm@240:         ys (map second verts)
rlm@229:         x0 (Math/floor (apply min xs))
rlm@229:         y0 (Math/floor (apply min ys))
rlm@229:         x1 (Math/ceil (apply max xs))
rlm@229:         y1 (Math/ceil (apply max ys))]
rlm@229:     [x0 y0 (- x1 x0) (- y1 y0)]))
rlm@229: 
rlm@229: (defn same-side?
rlm@229:   "Given the points p1 and p2 and the reference point ref, is point p
rlm@229:   on the same side of the line that goes through p1 and p2 as ref is?" 
rlm@229:   [p1 p2 ref p]
rlm@229:   (<=
rlm@229:    0
rlm@229:    (.dot 
rlm@229:     (.cross (.subtract p2 p1) (.subtract p p1))
rlm@229:     (.cross (.subtract p2 p1) (.subtract ref p1)))))
rlm@229: 
rlm@229: (defn inside-triangle?
rlm@229:   "Is the point inside the triangle?"
rlm@229:   {:author "Dylan Holmes"}
rlm@229:   [#^Triangle tri #^Vector3f p]
rlm@240:   (let [[vert-1 vert-2 vert-3] [(.get1 tri) (.get2 tri) (.get3 tri)]]
rlm@229:     (and
rlm@229:      (same-side? vert-1 vert-2 vert-3 p)
rlm@229:      (same-side? vert-2 vert-3 vert-1 p)
rlm@229:      (same-side? vert-3 vert-1 vert-2 p))))
rlm@229: #+end_src
rlm@229: 
rlm@228: * Physics Collision Objects
rlm@230: 
rlm@234: The "hairs" are actually =Rays= which extend  from a point on a
rlm@230: =Triangle= in the =Mesh= normal to the =Triangle's= surface.
rlm@230: 
rlm@226: * Headers 
rlm@231: 
rlm@231: #+name: touch-header
rlm@226: #+begin_src clojure
rlm@226: (ns cortex.touch
rlm@226:   "Simulate the sense of touch in jMonkeyEngine3. Enables any Geometry
rlm@226:   to be outfitted with touch sensors with density determined by a UV
rlm@226:   image. In this way a Geometry can know what parts of itself are
rlm@226:   touching nearby objects. Reads specially prepared blender files to
rlm@226:   construct this sense automatically."
rlm@226:   {:author "Robert McIntyre"}
rlm@226:   (:use (cortex world util sense))
rlm@226:   (:use clojure.contrib.def)
rlm@226:   (:import (com.jme3.scene Geometry Node Mesh))
rlm@226:   (:import com.jme3.collision.CollisionResults)
rlm@226:   (:import com.jme3.scene.VertexBuffer$Type)
rlm@226:   (:import (com.jme3.math Triangle Vector3f Vector2f Ray Matrix4f)))
rlm@226: #+end_src   
rlm@37: 
rlm@232: * Adding Touch to the Worm
rlm@232: 
rlm@232: #+name: test-touch
rlm@232: #+begin_src clojure
rlm@232: (ns cortex.test.touch
rlm@232:   (:use (cortex world util sense body touch))
rlm@232:   (:use cortex.test.body))
rlm@232: 
rlm@232: (cortex.import/mega-import-jme3)
rlm@232: 
rlm@232: (defn test-touch []
rlm@232:   (let [the-worm (doto (worm) (body!))
rlm@232:         touch (touch! the-worm)
rlm@232:         touch-display (view-touch)]
rlm@232:     (world (nodify [the-worm (floor)])
rlm@232:            standard-debug-controls
rlm@232:            
rlm@232:            (fn [world]
rlm@244:              (speed-up world)
rlm@232:              (light-up-everything world))
rlm@232: 
rlm@232:            (fn [world tpf]
rlm@246:              (touch-display 
rlm@246:               (map #(% (.getRootNode world)) touch))))))
rlm@232: #+end_src
rlm@228: * Source Listing
rlm@228: * Next
rlm@228: 
rlm@228: 
rlm@226: * COMMENT Code Generation
rlm@39: #+begin_src clojure :tangle ../src/cortex/touch.clj
rlm@231: <<touch-header>>
rlm@231: <<meta-data>>
rlm@231: <<triangles-1>>
rlm@231: <<triangles-3>>
rlm@231: <<triangles-4>>
rlm@231: <<sensors>>
rlm@231: <<kernel>>
rlm@231: <<visualization>>
rlm@0: #+end_src
rlm@0: 
rlm@232: 
rlm@68: #+begin_src clojure :tangle ../src/cortex/test/touch.clj 
rlm@232: <<test-touch>>
rlm@39: #+end_src
rlm@39: 
rlm@0: 
rlm@0: 
rlm@0:   
rlm@32: 
rlm@32: 
rlm@226: