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@229: 
rlm@229: 
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@229: Each geometry can have a single UV map which describes the position
rlm@229: and length of the "hairs" which will constitute its sense of
rlm@229: touch. This image path is stored under the "touch" key.  The image
rlm@229: itself is grayscale, with black meaning a hair length of 0 (no hair is
rlm@229: present) and white meaning a hair length of =scale=, which is a float
rlm@229: stored under the key "scale". If the pixel is gray then the resultant
rlm@229: hair length is linearly interpolated between 0 and =scale=.
rlm@229: 
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@228: #+end_src
rlm@156: 
rlm@229: 
rlm@229: ** TODO add image showing example touch-uv map
rlm@229: ** TODO add metadata display for worm
rlm@229: 
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@228: #+begin_src clojure
rlm@228: (defn triangles
rlm@228:   "Return a sequence of all the Triangles which compose a given
rlm@228:    Geometry." 
rlm@228:   [#^Geometry geom]
rlm@228:   (let
rlm@228:       [mesh (.getMesh geom)
rlm@228:        triangles (transient [])]
rlm@228:     (dorun
rlm@228:      (for [n (range (.getTriangleCount mesh))]
rlm@228:        (let [tri (Triangle.)]
rlm@228:          (.getTriangle mesh n tri)
rlm@228:         ;; (.calculateNormal tri)
rlm@228:         ;; (.calculateCenter tri)
rlm@228:          (conj! triangles tri))))
rlm@228:     (persistent! triangles)))
rlm@228:    
rlm@228: (defn mesh-triangle
rlm@228:   "Get the triangle specified by triangle-index from the mesh within
rlm@228:   bounds."
rlm@228:   [#^Mesh mesh triangle-index]
rlm@228:   (let [scratch (Triangle.)]
rlm@228:     (.getTriangle mesh triangle-index scratch)
rlm@228:     scratch))
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@228: (defn triangle-UV-coord
rlm@228:   "Get the UV-cooridnates of the triangle's verticies."
rlm@228:   [#^Mesh mesh width height triangle-index]
rlm@228:   (map (fn [[u v]] (vector (* width u) (* height v)))
rlm@228:        (map (partial vertex-UV-coord mesh)
rlm@228:             (triangle-vertex-indices mesh triangle-index))))
rlm@228: #+end_src
rlm@228: 
rlm@228: * Schrapnel Conversion Functions
rlm@229: 
rlm@229: It is convienent to treat a =Triangle= as a sequence of verticies, and
rlm@229: a =Vector2f= and =Vector3f= as a sequence of floats. These conversion
rlm@229: functions make this easy. If these classes implemented =Iterable= then
rlm@229: this code would not be necessary. Hopefully they will in the future.
rlm@229: 
rlm@228: #+begin_src clojure
rlm@228: (defn triangle-seq [#^Triangle tri]
rlm@228:   [(.get1 tri) (.get2 tri) (.get3 tri)])
rlm@228: 
rlm@228: (defn vector3f-seq [#^Vector3f v]
rlm@228:   [(.getX v) (.getY v) (.getZ v)])
rlm@228: 
rlm@228: (defn point->vector2f [[u v]]
rlm@228:   (Vector2f. u v))
rlm@228: 
rlm@228: (defn vector2f->vector3f [v]
rlm@228:   (Vector3f. (.getX v) (.getY v) 0))
rlm@228: 
rlm@228: (defn map-triangle [f #^Triangle tri]
rlm@228:   (Triangle.
rlm@228:    (f 0 (.get1 tri))
rlm@228:    (f 1 (.get2 tri))
rlm@228:    (f 2 (.get3 tri))))
rlm@228: 
rlm@228: (defn points->triangle
rlm@228:   "Convert a list of points into a triangle."
rlm@228:   [points]
rlm@228:   (apply #(Triangle. %1 %2 %3)
rlm@228:          (map (fn [point]
rlm@228:                 (let [point (vec point)]
rlm@228:                   (Vector3f. (get point 0 0)
rlm@228:                              (get point 1 0)
rlm@228:                              (get point 2 0))))
rlm@228:               (take 3 points))))
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@228: #+begin_src clojure
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@228:          (.set mat 3 row 1))))
rlm@228:     mat))
rlm@228: 
rlm@228: (defn triangle-transformation
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@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@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@229:   [uv-verts]
rlm@229:   (let [xs (map first uv-verts)
rlm@229:         ys (map second uv-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@229:   (let [[vert-1 vert-2 vert-3] (triangle-seq 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@229: 
rlm@229: 
rlm@229: * Sensor Related Functions
rlm@229: 
rlm@229: These functions analyze the touch-sensor-profile image convert the
rlm@229: location of each touch sensor from pixel coordinates to UV-coordinates
rlm@229: and XYZ-coordinates.
rlm@229: 
rlm@229: #+begin_src clojure
rlm@229: (defn sensors-in-triangle
rlm@229:   "Locate the touch sensors in the triangle, returning a map of their
rlm@229:    UV and geometry-relative coordinates."
rlm@229:   [image mesh tri-index]
rlm@229:   (let [width (.getWidth image)
rlm@229:         height (.getHeight image)
rlm@229:         UV-vertex-coords (triangle-UV-coord mesh width height tri-index)
rlm@229:         bounds (convex-bounds UV-vertex-coords)
rlm@229:         
rlm@229:         cutout-triangle (points->triangle UV-vertex-coords)
rlm@229:         UV-sensor-coords
rlm@229:         (filter (comp (partial inside-triangle? cutout-triangle)
rlm@229:                       (fn [[u v]] (Vector3f. u v 0)))
rlm@229:                 (white-coordinates image bounds))
rlm@229:         UV->geometry (triangle-transformation
rlm@229:                       cutout-triangle
rlm@229:                       (mesh-triangle mesh tri-index))
rlm@229:         geometry-sensor-coords
rlm@229:         (map (fn [[u v]] (.mult UV->geometry (Vector3f. u v 0)))
rlm@229:              UV-sensor-coords)]
rlm@229:   {:UV UV-sensor-coords :geometry geometry-sensor-coords}))
rlm@229: 
rlm@229: (defn-memo locate-feelers
rlm@229:   "Search the geometry's tactile UV profile for touch sensors,
rlm@229:    returning their positions in geometry-relative coordinates."
rlm@229:   [#^Geometry geo]
rlm@229:   (let [mesh (.getMesh geo)
rlm@229:         num-triangles (.getTriangleCount mesh)]
rlm@229:     (if-let [image (tactile-sensor-profile geo)]
rlm@229:       (map
rlm@229:        (partial sensors-in-triangle image mesh)
rlm@229:        (range num-triangles))
rlm@229:       (repeat (.getTriangleCount mesh) {:UV nil :geometry nil}))))
rlm@229: 
rlm@229: (defn-memo touch-topology
rlm@229:   "Return a sequence of vectors of the form [x y] describing the
rlm@229:    \"topology\" of the tactile sensors. Points that are close together
rlm@229:    in the touch-topology are generally close together in the simulation."
rlm@229:   [#^Gemoetry geo]
rlm@229:   (vec (collapse (reduce concat (map :UV (locate-feelers geo))))))
rlm@229: 
rlm@229: (defn-memo feeler-coordinates
rlm@229:   "The location of the touch sensors in world-space coordinates."
rlm@229:   [#^Geometry geo]
rlm@229:   (vec (map :geometry (locate-feelers geo))))
rlm@228: #+end_src
rlm@228: 
rlm@228: * Physics Collision Objects
rlm@228: #+begin_src clojure
rlm@228: (defn get-ray-origin
rlm@228:   "Return the origin which a Ray would have to have to be in the exact
rlm@228:   center of a particular Triangle in the Geometry in World
rlm@228:   Coordinates."
rlm@228:   [geom tri]
rlm@228:   (let [new (Vector3f.)]
rlm@228:     (.calculateCenter tri)
rlm@228:     (.localToWorld geom (.getCenter tri) new) new))
rlm@228: 
rlm@228: (defn get-ray-direction
rlm@228:   "Return the direction which a Ray would have to have to be to point
rlm@228:   normal to the Triangle, in coordinates relative to the center of the
rlm@228:   Triangle."
rlm@228:   [geom tri]
rlm@228:   (let [n+c (Vector3f.)]
rlm@228:     (.calculateNormal tri)
rlm@228:     (.calculateCenter tri)
rlm@228:     (.localToWorld
rlm@228:      geom
rlm@228:      (.add (.getCenter tri) (.getNormal tri)) n+c)
rlm@228:     (.subtract n+c (get-ray-origin geom tri))))
rlm@228: #+end_src
rlm@228: 
rlm@228: 
rlm@228: * Skin Creation
rlm@228: 
rlm@228: #+begin_src clojure
rlm@178: (defn touch-fn
rlm@178:   "Returns a function which returns tactile sensory data when called
rlm@178:    inside a running simulation."
rlm@178:   [#^Geometry geo]
rlm@156:   (let [feeler-coords (feeler-coordinates geo)
rlm@156:         tris (triangles geo)
rlm@156:         limit 0.1
rlm@156:         ;;results (CollisionResults.)
rlm@156:         ]
rlm@156:     (if (empty? (touch-topology geo))
rlm@156:       nil
rlm@156:       (fn [node]
rlm@156:         (let [sensor-origins 
rlm@156:               (map
rlm@156:                #(map (partial local-to-world geo) %)
rlm@156:                feeler-coords)
rlm@156:               triangle-normals 
rlm@156:               (map (partial get-ray-direction geo)
rlm@156:                    tris)
rlm@156:               rays
rlm@156:               (flatten
rlm@156:                (map (fn [origins norm]
rlm@156:                       (map #(doto (Ray. % norm)
rlm@156:                               (.setLimit limit)) origins))
rlm@156:                     sensor-origins triangle-normals))]
rlm@156:           (vector
rlm@156:            (touch-topology geo)
rlm@156:            (vec
rlm@156:             (for [ray rays]
rlm@156:               (do
rlm@156:                 (let [results (CollisionResults.)]
rlm@156:                   (.collideWith node ray results)
rlm@156:                   (let [touch-objects
rlm@156:                         (filter #(not (= geo (.getGeometry %)))
rlm@156:                                 results)]
rlm@156:                     (- 255
rlm@156:                        (if (empty? touch-objects) 255
rlm@156:                            (rem 
rlm@156:                             (int
rlm@156:                              (* 255 (/ (.getDistance
rlm@156:                                         (first touch-objects)) limit)))
rlm@156:                             256))))))))))))))
rlm@156:   
rlm@178: (defn touch! 
rlm@178:   "Endow the creature with the sense of touch. Returns a sequence of
rlm@178:    functions, one for each body part with a tactile-sensor-proile,
rlm@178:    each of which when called returns sensory data for that body part."
rlm@178:   [#^Node creature]
rlm@178:   (filter
rlm@178:    (comp not nil?)
rlm@178:    (map touch-fn
rlm@178:         (filter #(isa? (class %) Geometry)
rlm@178:                 (node-seq creature)))))
rlm@228: #+end_src
rlm@156: 
rlm@228: * Visualizing Touch
rlm@228: 
rlm@228: #+begin_src clojure
rlm@188: (defn view-touch 
rlm@189:   "Creates a function which accepts a list of  touch sensor-data and
rlm@189:    displays each element to the screen."
rlm@188:   []
rlm@187:   (view-sense
rlm@187:    (fn 
rlm@187:      [[coords sensor-data]]
rlm@187:      (let [image (points->image coords)]
rlm@187:        (dorun
rlm@187:         (for [i (range (count coords))]
rlm@187:           (.setRGB image ((coords i) 0) ((coords i) 1)
rlm@187:                    (gray (sensor-data i)))))
rlm@188:        image))))
rlm@37: #+end_src
rlm@37: 
rlm@226: * Headers 
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@228: 
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@0: <<skin-main>>
rlm@0: #+end_src
rlm@0: 
rlm@68: #+begin_src clojure :tangle ../src/cortex/test/touch.clj 
rlm@39: #+end_src
rlm@39: 
rlm@0: 
rlm@0: 
rlm@0:   
rlm@32: 
rlm@32: 
rlm@226: