#+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

 

 My creatures need to be able to feel their environments. The idea here

 is to create thousands of small /touch receptors/ along the geometries

 which make up the creature's body. The number of touch receptors in a

 given area is determined by how complicated that area is, as

 determined by the total number of triangles in that region. This way,

 complicated regions like the hands/face, etc. get more touch receptors

 than simpler areas of the body.

 

 #+name: skin-main

 #+begin_src clojure

 (ns cortex.touch

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

   to be outfitted with touch sensors with density proportional to the

   density of triangles along the surface of the Geometry. Enables a

   Geometry to know what parts of itself are touching nearby objects."

   {: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)))

    

 (defn triangles

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

   Geometry." 

   [#^Geometry geom]

   (let

       [mesh (.getMesh geom)

        triangles (transient [])]

     (dorun

      (for [n (range (.getTriangleCount mesh))]

        (let [tri (Triangle.)]

          (.getTriangle mesh n tri)

         ;; (.calculateNormal tri)

         ;; (.calculateCenter tri)

          (conj! triangles tri))))

     (persistent! triangles)))

    

 (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))))

 

 ;; Every Mesh has many triangles, each with its own index.

 ;; Every vertex has its own index as well.

 

 (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 triangle

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

   bounds."

   [#^Mesh mesh triangle-index]

   (let [scratch (Triangle.)]

     (.getTriangle mesh triangle-index scratch)

     scratch))

 

 (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 triangle-UV-coord

   "Get the uv-cooridnates of the triangle's verticies."

   [#^Mesh mesh width height triangle-index]

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

        (map (partial vertex-UV-coord mesh)

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

   

 (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 triangle-seq [#^Triangle tri]

   [(.get1 tri) (.get2 tri) (.get3 tri)])

 

 (defn vector3f-seq [#^Vector3f v]

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

 

 (defn inside-triangle?

   "Is the point inside the triangle?"

   {:author "Dylan Holmes"}

   [#^Triangle tri #^Vector3f p]

   (let [[vert-1 vert-2 vert-3] (triangle-seq 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))))

 

 (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 triangle-transformation

   "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))))

 

 (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))))

 

 (defn convex-bounds

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

    vector of integers [left top width height]."

   [uv-verts]

   (let [xs (map first uv-verts)

         ys (map second uv-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 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

                       (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))))

 

 (defn touch-fn

   "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 0.1

         ;;results (CollisionResults.)

         ]

     (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)]

                     (- 255

                        (if (empty? touch-objects) 255

                            (rem 

                             (int

                              (* 255 (/ (.getDistance

                                         (first touch-objects)) limit)))

                             256))))))))))))))

   

 (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-fn

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

                 (node-seq creature)))))

 

 (defn gray-scale

   "Create a gray RGB pixel with R, G, and B set to 'num"

   [num]

   (+ num

      (bit-shift-left num 8)

      (bit-shift-left num 16)))

 

 (defn view-touch 

   "Creates a function which accepts touch sensor-data and displays it

    as BufferedImages in JFrames."

   []

   (let

       [windows (atom [])

        display-single-touch

        (fn 

          [[coords sensor-data] display]

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

            (dorun

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

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

                        (gray-scale (sensor-data i)))))

            (display image)))]

     (fn [data]

       (if (> (count data) (count @windows))

         (reset! windows (map (fn [_] (view-image))

                              (range (count data)))))

       (dorun (map display-single-touch data @windows)))))

         

 

 

 #+end_src

 

 

 * Example

 

 #+name: touch-test

 #+begin_src clojure 

 (ns cortex.test.touch

   (:use (cortex world util touch))

   (:import

    com.jme3.scene.shape.Sphere

    com.jme3.math.ColorRGBA

    com.jme3.math.Vector3f

    com.jme3.material.RenderState$BlendMode

    com.jme3.renderer.queue.RenderQueue$Bucket

    com.jme3.scene.shape.Box

    com.jme3.scene.Node))

 

 (defn ray-origin-debug

   [ray color]

   (make-shape

    (assoc base-shape

      :shape (Sphere. 5 5 0.05)

      :name "arrow"

      :color color

      :texture false

      :physical? false

      :position

      (.getOrigin ray))))

 

 (defn ray-debug [ray color]

   (make-shape

    (assoc

        base-shape

      :name "debug-ray"

      :physical? false

      :shape (com.jme3.scene.shape.Line.

              (.getOrigin ray)

              (.add

               (.getOrigin ray)

               (.mult (.getDirection ray)

                      (float (.getLimit ray))))))))

              

 

 (defn contact-color [contacts]

   (case contacts

     0 ColorRGBA/Gray

     1  ColorRGBA/Red 

     2  ColorRGBA/Green 

     3  ColorRGBA/Yellow 

     4  ColorRGBA/Orange 

     5  ColorRGBA/Red 

     6  ColorRGBA/Magenta 

     7  ColorRGBA/Pink 

     8  ColorRGBA/White))

 

 (defn update-ray-debug [node ray contacts]

   (let [origin (.getChild node 0)]

     (.setLocalTranslation origin (.getOrigin ray))

     (.setColor (.getMaterial origin) "Color" (contact-color contacts))))

 

 (defn init-node

   [debug-node rays]

     (.detachAllChildren debug-node)

     (dorun 

      (for [ray rays]

        (do

          (.attachChild

           debug-node 

           (doto (Node.)

             (.attachChild (ray-origin-debug ray ColorRGBA/Gray))

             (.attachChild (ray-debug ray ColorRGBA/Gray))

             ))))))

 

 (defn manage-ray-debug-node [debug-node geom touch-data limit]

   (let [rays (normal-rays limit geom)]

     (if (not= (count (.getChildren debug-node)) (count touch-data))

       (init-node debug-node rays))

     (dorun 

      (for [n (range (count touch-data))]

        (update-ray-debug

         (.getChild debug-node n) (nth rays n) (nth touch-data n))))))

 

 (defn transparent-sphere []

   (doto

       (make-shape

        (merge base-shape

 	      {:position (Vector3f. 0 2 0)

 	       :name "the blob."

 	       :material "Common/MatDefs/Misc/Unshaded.j3md"

 	       :texture "Textures/purpleWisp.png"

 	       :physical? true

 	       :mass 70

 	       :color ColorRGBA/Blue

 	       :shape (Sphere. 10 10 1)}))

     (-> (.getMaterial)

 	(.getAdditionalRenderState)

 	(.setBlendMode RenderState$BlendMode/Alpha))

     (.setQueueBucket RenderQueue$Bucket/Transparent)))

 

 (defn transparent-box []

     (doto

       (make-shape

        (merge base-shape

 	      {:position (Vector3f. 0 2 0)

 	       :name "box"

 	       :material "Common/MatDefs/Misc/Unshaded.j3md"

 	       :texture "Textures/purpleWisp.png"

 	       :physical? true

 	       :mass 70

 	       :color ColorRGBA/Blue

 	       :shape (Box. 1 1 1)}))

     (-> (.getMaterial)

 	(.getAdditionalRenderState)

 	(.setBlendMode RenderState$BlendMode/Alpha))

     (.setQueueBucket RenderQueue$Bucket/Transparent)))

 

 (defn transparent-floor []

   (doto

       (box 5 0.2 5  :mass 0 :position (Vector3f. 0 -2 0)

            :material "Common/MatDefs/Misc/Unshaded.j3md"

            :texture "Textures/redWisp.png"

            :name "floor")

     (-> (.getMaterial)

         (.getAdditionalRenderState)

         (.setBlendMode RenderState$BlendMode/Alpha))

     (.setQueueBucket RenderQueue$Bucket/Transparent)))

 

 (defn test-skin 

   "Testing touch:

    you should see a ball which responds to the table

    and whatever balls hit it."

   []

   (let [b

         ;;(transparent-box)

         (transparent-sphere)

         ;;(sphere)

         f (transparent-floor)

         debug-node (Node.)

         node      (doto (Node.) (.attachChild b) (.attachChild f))

         root-node (doto (Node.) (.attachChild node)

                         (.attachChild debug-node))

         ]

     

     (world

      root-node

      {"key-return" (fire-cannon-ball node)}

      (fn [world]

        ;;  (Capture/SimpleCaptureVideo

        ;;   world 

        ;;   (file-str "/home/r/proj/cortex/tmp/blob.avi"))

        ;;  (no-logging)

        ;;(enable-debug world)

        ;;  (set-accuracy world (/ 1 60))

        )

      

      (fn [& _]

        (let [sensitivity 0.2

              touch-data (touch-percieve sensitivity b node)]

          (manage-ray-debug-node debug-node b touch-data sensitivity))

        ))))

 

 

 #+end_src

 

 

 

 

   

 * COMMENT code generation

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

 <<skin-main>>

 #+end_src

 

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

 <<touch-test>>

 #+end_src