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: 
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@229: ** TODO add image showing example touch-uv map
rlm@229: ** TODO add metadata display for worm
rlm@229: 
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@233:    (fn 
rlm@233:      [[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@233:                    (apply touch->gray (sensor-data i)))))
rlm@233:        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@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@244:              (touch-display (map #(% (.getRootNode world)) touch))
rlm@243:              ))))
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: