#+title: Simulated Sense of Touch

 #+author: Robert McIntyre

 #+email: rlm@mit.edu

 #+description: Simulated touch for AI research using JMonkeyEngine and clojure.

 #+keywords: simulation, tactile sense, jMonkeyEngine3, clojure

 #+SETUPFILE: ../../aurellem/org/setup.org

 #+INCLUDE: ../../aurellem/org/level-0.org

 

 

 

 * Touch

 

 Touch is critical to navigation and spatial reasoning and as such I

 need a simulated version of it to give to my AI creatures.

 

 However, touch in my virtual can not exactly correspond to human touch

 because my creatures are made out of completely rigid segments that

 don't deform like human skin.

 

 Human skin has a wide array of touch sensors, each of which speciliaze

 in detecting different vibrational modes and pressures. These sensors

 can integrate a vast expanse of skin (i.e. your entire palm), or a

 tiny patch of skin at the tip of your finger. The hairs of the skin

 help detect objects before they even come into contact with the skin

 proper. 

 

 Instead of measuring deformation or vibration, I surround each rigid

 part with a plenitude of hair-like objects which do not interact with

 the physical world. Physical objects can pass through them with no

 effect. The hairs are able to measure contact with other objects, and

 constantly report how much of their extent is covered. So, even though

 the creature's body parts do not deform, the hairs create a margin

 around those body parts which achieves a sense of touch which is a

 hybrid between a human's sense of deformation and sense from hairs.

 

 Implementing touch in jMonkeyEngine follows a different techinal route

 than vision and hearing. Those two senses piggybacked off

 jMonkeyEngine's 3D audio and video rendering subsystems. To simulate

 Touch, I use jMonkeyEngine's physics system to execute many small

 collision detections, one for each "hair". The placement of the

 "hairs" is determined by a UV-mapped image which shows where each hair

 should be on the 3D surface of the body.

 

 

 * Defining Touch Meta-Data in Blender

 

 Each geometry can have a single UV map which describes the position

 and length of the "hairs" which will constitute its sense of

 touch. This image path is stored under the "touch" key.  The image

 itself is grayscale, with black meaning a hair length of 0 (no hair is

 present) and white meaning a hair length of =scale=, which is a float

 stored under the key "scale". If the pixel is gray then the resultant

 hair length is linearly interpolated between 0 and =scale=. I call

 these "hairs" /feelers/.

 

 #+name: meta-data

 #+begin_src clojure

 (defn tactile-sensor-profile

   "Return the touch-sensor distribution image in BufferedImage format,

    or nil if it does not exist."

   [#^Geometry obj]

   (if-let [image-path (meta-data obj "touch")]

     (load-image image-path)))

 

 (defn tactile-scale

   "Return the maximum length of a hair.  All hairs are scalled between

    0.0 and this length, depending on their color. Black is 0, and

    white is maximum length, and everything in between is scalled

    linearlly. Default scale is 0.01 jMonkeyEngine units."

   [#^Geometry obj]

   (if-let [scale (meta-data obj "scale")]

     scale 0.1))

 #+end_src

 

 ** TODO add image showing example touch-uv map

 ** TODO add metadata display for worm

 

 

 * Skin Creation

 * TODO get the actual lengths for each hair

 

 #+begin_src clojure

 pixel-triangles

         xyz-triangles

         conversions (map triangles->affine-transform pixel-triangles

                          xyz-triangles)

 

 #+end_src

 

 

 =(touch-kernel)= generates the functions which implement the sense of

 touch for a creature. These functions must do 6 things to obtain touch

 data.

 

   - Get the tactile profile image and scale paramaters which describe

     the layout of feelers along the object's surface.

     =(tactile-sensor-profile)=, =(tactile-scale)=

 

   - Get the lengths of each feeler by analyzing the color of the

     pixels in the tactile profile image.

     NOT IMPLEMENTED YET

 

   - Find the triangles which make up the mesh in pixel-space and in

     world-space.

     =(triangles)= =(pixel-triangles)=

     

   - Find the coordinates of each pixel in pixel space. These

     coordinates are used to make the touch-topology.

     =(feeler-pixel-coords)=

 

   - Find the coordinates of each pixel in world-space. These

     coordinates are the origins of the feelers. =(feeler-origins)=

     

   - Calculate the normals of the triangles in world space, and add

     them to each of the origins of the feelers. These are the

     normalized coordinates of the tips of the feelers.

     For both of these, =(feeler-tips)=

 

   - Generate some sort of topology for the sensors.

     =(touch-topology)=

 

 #+begin_src clojure

 

 

 

 

 #+end_src

 

 

 

 #+name: kernel

 #+begin_src clojure

 (in-ns 'cortex.touch)

 

 (declare touch-topology feelers set-ray)

 

 (defn set-ray [#^Ray ray #^Matrix4f transform

                #^Vector3f origin #^Vector3f tip

                length]

   (.setOrigin ray (.mult transform origin))

   (.setDirection  ray (.subtract

                        (.mult transform tip)

                        (.getOrigin ray)))

   (.setLimit ray length)

   ray)

 

 

 (defn touch-kernel

   "Constructs a function which will return tactile sensory data from

    'geo when called from inside a running simulation"

   [#^Geometry geo]

   (let [profile (tactile-sensor-profile geo)

         ray-reference-origins (feeler-origins geo profile)

         ray-reference-tips (feeler-tips geo profile)

         ray-lengths (repeat 9000 0.1)

         current-rays (map (fn [] (Ray.)) ray-reference-origins)

         topology (touch-topology geo profile)]

     (if (empty? ray-reference-origins) nil

         (fn [node]

           (let [transform (.getWorldMatrix geo)]

             (dorun

              (map (fn [ray ref-origin ref-tip length]

                     (set-ray ray transform ref-origin ref-tip length))

                   current-rays ray-reference-origins

                   ray-reference-tips ray-lengths))

             (vector

              topology

              (vec

               (for [ray current-rays]

                 (do

                   (let [results (CollisionResults.)]

                     (.collideWith node ray results)

                     (let [touch-objects

                           (filter #(not (= geo (.getGeometry %)))

                                   results)]

                       [(if (empty? touch-objects)

                          (.getLimit ray)

                          (.getDistance (first touch-objects)))

                        (.getLimit ray)])))))))))))

 

 (defn touch-kernel*

   "Returns a function which returns tactile sensory data when called

    inside a running simulation."

   [#^Geometry geo]

   (let [feeler-coords (feeler-coordinates geo)

         tris (triangles geo)

         limit (tactile-scale geo)]

     (if (empty? (touch-topology geo))

       nil

       (fn [node]

         (let [sensor-origins 

               (map

                #(map (partial local-to-world geo) %)

                feeler-coords)

               triangle-normals 

               (map (partial get-ray-direction geo)

                    tris)

               rays

               (flatten

                (map (fn [origins norm]

                       (map #(doto (Ray. % norm)

                               (.setLimit limit)) origins))

                     sensor-origins triangle-normals))]

           (vector

            (touch-topology geo)

            (vec

             (for [ray rays]

               (do

                 (let [results (CollisionResults.)]

                   (.collideWith node ray results)

                   (let [touch-objects

                         (filter #(not (= geo (.getGeometry %)))

                                 results)]

                     [(if (empty? touch-objects)

                        limit (.getDistance (first touch-objects)))

                      limit])))))))))))

 

 (defn touch! 

   "Endow the creature with the sense of touch. Returns a sequence of

    functions, one for each body part with a tactile-sensor-proile,

    each of which when called returns sensory data for that body part."

   [#^Node creature]

   (filter

    (comp not nil?)

    (map touch-kernel

         (filter #(isa? (class %) Geometry)

                 (node-seq creature)))))

 #+end_src

 

 * Sensor Related Functions

 

 These functions analyze the touch-sensor-profile image convert the

 location of each touch sensor from pixel coordinates to UV-coordinates

 and XYZ-coordinates.

 

 #+name: sensors

 #+begin_src clojure

 (in-ns 'cortex.touch)

 

 (defn feeler-pixel-coords

  "Returns the coordinates of the feelers in pixel space in lists, one

   list for each triangle, ordered in the same way as (triangles) and

   (pixel-triangles)."

  [#^Geometry geo image]

  (map 

   (fn [pixel-triangle]

     (filter

      (fn [coord]

        (inside-triangle? (->triangle pixel-triangle)

                          (->vector3f coord)))

        (white-coordinates image (convex-bounds pixel-triangle))))

   (pixel-triangles geo image)))

 

 (defn feeler-world-coords [#^Geometry geo image]

   (let [transforms

         (map #(triangles->affine-transform

                (->triangle %1) (->triangle %2))

              (pixel-triangles geo image)

              (triangles geo))]

     (map (fn [transform coords]

               (map #(.mult transform (->vector3f %)) coords))

             transforms (feeler-pixel-coords geo image))))

 

 (defn feeler-origins [#^Geometry geo image]

    (reduce concat (feeler-world-coords geo image)))

 

 (defn feeler-tips [#^Geometry geo image]

   (let [world-coords (feeler-world-coords geo image)

         normals

         (map

          (fn [triangle]

            (.calculateNormal triangle)

            (.clone (.getNormal triangle)))

          (map ->triangle (triangles geo)))]

 

     (mapcat (fn [origins normal]

               (map #(.add % normal) origins))

             world-coords normals)))

     

 

 (defn touch-topology [#^Geometry geo image]

   (collapse (feeler-pixel-coords geo image)))

   

 

 (defn sensors-in-triangle

   "Locate the touch sensors in the triangle, returning a map of their

    UV and geometry-relative coordinates."

   [image mesh tri-index]

   (let [width (.getWidth image)

         height (.getHeight image)

         UV-vertex-coords (triangle-UV-coord mesh width height tri-index)

         bounds (convex-bounds UV-vertex-coords)

         

         cutout-triangle (points->triangle UV-vertex-coords)

         UV-sensor-coords

         (filter (comp (partial inside-triangle? cutout-triangle)

                       (fn [[u v]] (Vector3f. u v 0)))

                 (white-coordinates image bounds))

         UV->geometry (triangle-transformation

                       cutout-triangle

                       (mesh-triangle mesh tri-index))

         geometry-sensor-coords

         (map (fn [[u v]] (.mult UV->geometry (Vector3f. u v 0)))

              UV-sensor-coords)]

   {:UV UV-sensor-coords :geometry geometry-sensor-coords}))

 

 (defn-memo locate-feelers

   "Search the geometry's tactile UV profile for touch sensors,

    returning their positions in geometry-relative coordinates."

   [#^Geometry geo]

   (let [mesh (.getMesh geo)

         num-triangles (.getTriangleCount mesh)]

     (if-let [image (tactile-sensor-profile geo)]

       (map

        (partial sensors-in-triangle image mesh)

        (range num-triangles))

       (repeat (.getTriangleCount mesh) {:UV nil :geometry nil}))))

 

 (defn-memo touch-topology

   "Return a sequence of vectors of the form [x y] describing the

    \"topology\" of the tactile sensors. Points that are close together

    in the touch-topology are generally close together in the simulation."

   [#^Gemoetry geo]

   (vec (collapse (reduce concat (map :UV (locate-feelers geo))))))

 

 (defn-memo feeler-coordinates

   "The location of the touch sensors in world-space coordinates."

   [#^Geometry geo]

   (vec (map :geometry (locate-feelers geo))))

 #+end_src

 

 

 

 

 * Visualizing Touch

 #+name: visualization

 #+begin_src clojure

 (in-ns 'cortex.touch)

 

 (defn touch->gray

   "Convert a pair of [distance, max-distance] into a grayscale pixel" 

   [distance max-distance]

   (gray 

    (- 255

       (rem 

        (int

         (* 255 (/ distance max-distance)))

        256))))

 

 (defn view-touch 

   "Creates a function which accepts a list of  touch sensor-data and

    displays each element to the screen."

   []

   (view-sense

    (fn 

      [[coords sensor-data]]

      (let [image (points->image coords)]

        (dorun

         (for [i (range (count coords))]

           (.setRGB image ((coords i) 0) ((coords i) 1)

                    (apply touch->gray (sensor-data i)))))

        image))))

 #+end_src

 

 

 

 * Triangle Manipulation Functions

 

 The rigid bodies which make up a creature have an underlying

 =Geometry=, which is a =Mesh= plus a =Material= and other important

 data involved with displaying the body. 

 

 A =Mesh= is composed of =Triangles=, and each =Triangle= has three

 verticies which have coordinates in XYZ space and UV space.

  

 Here, =(triangles)= gets all the triangles which compose a mesh, and

 =(triangle-UV-coord)= returns the the UV coordinates of the verticies

 of a triangle.

 

 #+name: triangles-1

 #+begin_src clojure

 (in-ns 'cortex.touch)

 

 (defn vector3f-seq [#^Vector3f v]

   [(.getX v) (.getY v) (.getZ v)])

 

 (defn triangle-seq [#^Triangle tri]

   [(vector3f-seq (.get1 tri))

    (vector3f-seq (.get2 tri))

    (vector3f-seq (.get3 tri))])

 

 (defn ->vector3f 

   ([coords] (Vector3f. (nth coords 0 0)

                        (nth coords 1 0)

                        (nth coords 2 0))))

 

 (defn ->triangle [points]

   (apply #(Triangle. %1 %2 %3) (map ->vector3f points)))

 

 (defn triangle

   "Get the triangle specified by triangle-index from the mesh within

   bounds."

   [#^Geometry geo triangle-index]

   (triangle-seq

    (let [scratch (Triangle.)]

      (.getTriangle (.getMesh geo) triangle-index scratch) scratch)))

 

 (defn triangles

   "Return a sequence of all the Triangles which compose a given

    Geometry." 

   [#^Geometry geo]

   (map (partial triangle geo) (range (.getTriangleCount (.getMesh geo)))))

 

 (defn triangle-vertex-indices

   "Get the triangle vertex indices of a given triangle from a given

    mesh."

   [#^Mesh mesh triangle-index]

   (let [indices (int-array 3)]

     (.getTriangle mesh triangle-index indices)

     (vec indices)))

 

 (defn vertex-UV-coord

   "Get the UV-coordinates of the vertex named by vertex-index"

   [#^Mesh mesh vertex-index]

   (let [UV-buffer

         (.getData

          (.getBuffer

           mesh

           VertexBuffer$Type/TexCoord))]

     [(.get UV-buffer (* vertex-index 2))

      (.get UV-buffer (+ 1 (* vertex-index 2)))]))

 

 (defn pixel-triangle [#^Geometry geo image index]

   (let [mesh (.getMesh geo)

         width (.getWidth image)

         height (.getHeight image)]

     (vec (map (fn [[u v]] (vector (* width u) (* height v)))

               (map (partial vertex-UV-coord mesh)

                    (triangle-vertex-indices mesh index))))))

 

 (defn pixel-triangles [#^Geometry geo image]

  (let [height (.getHeight image)

        width (.getWidth image)]

    (map (partial pixel-triangle geo image)

         (range (.getTriangleCount (.getMesh geo))))))

 

 #+end_src

 

 * Triangle Affine Transforms

 

 The position of each hair is stored in a 2D image in UV

 coordinates. To place the hair in 3D space we must convert from UV

 coordinates to XYZ coordinates. Each =Triangle= has coordinates in

 both UV-space and XYZ-space, which defines a unique [[http://mathworld.wolfram.com/AffineTransformation.html ][Affine Transform]]

 for translating any coordinate within the UV triangle to the

 cooresponding coordinate in the XYZ triangle.

 

 #+name: triangles-3

 #+begin_src clojure

 (defn triangle->matrix4f

   "Converts the triangle into a 4x4 matrix: The first three columns

    contain the vertices of the triangle; the last contains the unit

    normal of the triangle. The bottom row is filled with 1s."

   [#^Triangle t]

   (let [mat (Matrix4f.)

         [vert-1 vert-2 vert-3]

         ((comp vec map) #(.get t %) (range 3))

         unit-normal (do (.calculateNormal t)(.getNormal t))

         vertices [vert-1 vert-2 vert-3 unit-normal]]

     (dorun 

      (for [row (range 4) col (range 3)]

        (do

          (.set mat col row (.get (vertices row)col))

          (.set mat 3 row 1))))

     mat))

 

 (defn triangles->affine-transform

   "Returns the affine transformation that converts each vertex in the

    first triangle into the corresponding vertex in the second

    triangle."

   [#^Triangle tri-1 #^Triangle tri-2]

   (.mult 

    (triangle->matrix4f tri-2)

    (.invert (triangle->matrix4f tri-1))))

 #+end_src

 

 

 * Schrapnel Conversion Functions

 

 It is convienent to treat a =Triangle= as a sequence of verticies, and

 a =Vector2f= and =Vector3f= as a sequence of floats. These conversion

 functions make this easy. If these classes implemented =Iterable= then

 this code would not be necessary. Hopefully they will in the future.

 

 #+name: triangles-2

 #+begin_src clojure

 (defn point->vector2f [[u v]]

   (Vector2f. u v))

 

 (defn vector2f->vector3f [v]

   (Vector3f. (.getX v) (.getY v) 0))

 

 (defn map-triangle [f #^Triangle tri]

   (Triangle.

    (f 0 (.get1 tri))

    (f 1 (.get2 tri))

    (f 2 (.get3 tri))))

 

 (defn points->triangle

   "Convert a list of points into a triangle."

   [points]

   (apply #(Triangle. %1 %2 %3)

          (map (fn [point]

                 (let [point (vec point)]

                   (Vector3f. (get point 0 0)

                              (get point 1 0)

                              (get point 2 0))))

               (take 3 points))))

 #+end_src

 

 

 * Triangle Boundaries

   

 For efficiency's sake I will divide the UV-image into small squares

 which inscribe each UV-triangle, then extract the points which lie

 inside the triangle and map them to 3D-space using

 =(triangle-transform)= above. To do this I need a function,

 =(inside-triangle?)=, which determines whether a point is inside a

 triangle in 2D UV-space.

 

 #+name: triangles-4

 #+begin_src clojure

 (defn convex-bounds

   "Returns the smallest square containing the given vertices, as a

    vector of integers [left top width height]."

   [verts]

   (let [xs (map first verts)

         ys (map second verts)

         x0 (Math/floor (apply min xs))

         y0 (Math/floor (apply min ys))

         x1 (Math/ceil (apply max xs))

         y1 (Math/ceil (apply max ys))]

     [x0 y0 (- x1 x0) (- y1 y0)]))

 

 (defn same-side?

   "Given the points p1 and p2 and the reference point ref, is point p

   on the same side of the line that goes through p1 and p2 as ref is?" 

   [p1 p2 ref p]

   (<=

    0

    (.dot 

     (.cross (.subtract p2 p1) (.subtract p p1))

     (.cross (.subtract p2 p1) (.subtract ref p1)))))

 

 (defn inside-triangle?

   "Is the point inside the triangle?"

   {:author "Dylan Holmes"}

   [#^Triangle tri #^Vector3f p]

   (let [[vert-1 vert-2 vert-3] [(.get1 tri) (.get2 tri) (.get3 tri)]]

     (and

      (same-side? vert-1 vert-2 vert-3 p)

      (same-side? vert-2 vert-3 vert-1 p)

      (same-side? vert-3 vert-1 vert-2 p))))

 #+end_src

 

 #+results: triangles-4

 : #'cortex.touch/inside-triangle?

 

 

 * Physics Collision Objects

 

 The "hairs" are actually =Rays= which extend  from a point on a

 =Triangle= in the =Mesh= normal to the =Triangle's= surface.

 

 #+name: rays

 #+begin_src clojure

 (defn get-ray-origin

   "Return the origin which a Ray would have to have to be in the exact

   center of a particular Triangle in the Geometry in World

   Coordinates."

   [geom tri]

   (let [new (Vector3f.)]

     (.calculateCenter tri)

     (.localToWorld geom (.getCenter tri) new) new))

 

 (defn get-ray-direction

   "Return the direction which a Ray would have to have to be to point

   normal to the Triangle, in coordinates relative to the center of the

   Triangle."

   [geom tri]

   (let [n+c (Vector3f.)]

     (.calculateNormal tri)

     (.calculateCenter tri)

     (.localToWorld

      geom

      (.add (.getCenter tri) (.getNormal tri)) n+c)

     (.subtract n+c (get-ray-origin geom tri))))

 #+end_src

 * Headers 

 

 #+name: touch-header

 #+begin_src clojure

 (ns cortex.touch

   "Simulate the sense of touch in jMonkeyEngine3. Enables any Geometry

   to be outfitted with touch sensors with density determined by a UV

   image. In this way a Geometry can know what parts of itself are

   touching nearby objects. Reads specially prepared blender files to

   construct this sense automatically."

   {:author "Robert McIntyre"}

   (:use (cortex world util sense))

   (:use clojure.contrib.def)

   (:import (com.jme3.scene Geometry Node Mesh))

   (:import com.jme3.collision.CollisionResults)

   (:import com.jme3.scene.VertexBuffer$Type)

   (:import (com.jme3.math Triangle Vector3f Vector2f Ray Matrix4f)))

 #+end_src   

 

 * Adding Touch to the Worm

 

 #+name: test-touch

 #+begin_src clojure

 (ns cortex.test.touch

   (:use (cortex world util sense body touch))

   (:use cortex.test.body))

 

 (cortex.import/mega-import-jme3)

 

 (defn test-touch []

   (let [the-worm (doto (worm) (body!))

         touch (touch! the-worm)

         touch-display (view-touch)]

     (world (nodify [the-worm (floor)])

            standard-debug-controls

            

            (fn [world]

              (light-up-everything world))

 

            (fn [world tpf]

              (touch-display (map #(% (.getRootNode world)) touch))))))

 #+end_src

 * Source Listing

 * Next

 

 

 * COMMENT Code Generation

 #+begin_src clojure :tangle ../src/cortex/touch.clj

 <<touch-header>>

 <<meta-data>>

 <<triangles-1>>

 <<triangles-2>>

 <<triangles-3>>

 <<triangles-4>>

 <<sensors>>

 <<rays>>

 <<kernel>>

 <<visualization>>

 #+end_src

 

 

 #+begin_src clojure :tangle ../src/cortex/test/touch.clj 

 <<test-touch>>

 #+end_src