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@39: 
rlm@37: * Touch
rlm@0: 
rlm@37: My creatures need to be able to feel their environments. The idea here
rlm@43: is to create thousands of small /touch receptors/ along the geometries
rlm@43: which make up the creature's body. The number of touch receptors in a
rlm@43: given area is determined by how complicated that area is, as
rlm@43: determined by the total number of triangles in that region. This way,
rlm@43: complicated regions like the hands/face, etc. get more touch receptors
rlm@43: than simpler areas of the body.
rlm@0: 
rlm@66: #+name: skin-main
rlm@0: #+begin_src clojure
rlm@37: (ns cortex.touch
rlm@37:   "Simulate the sense of touch in jMonkeyEngine3. Enables any Geometry
rlm@37:   to be outfitted with touch sensors with density proportional to the
rlm@37:   density of triangles along the surface of the Geometry. Enables a
rlm@37:   Geometry to know what parts of itself are touching nearby objects."
rlm@37:   {:author "Robert McIntyre"}
rlm@156:   (:use (cortex world util sense))
rlm@37:   (:import com.jme3.scene.Geometry)
rlm@39:   (:import com.jme3.collision.CollisionResults)
rlm@156:   (:import jme3tools.converters.ImageToAwt)
rlm@39:   (:import (com.jme3.math Triangle Vector3f Ray)))
rlm@37:    
rlm@156: (use 'clojure.contrib.def)
rlm@156: (cortex.import/mega-import-jme3)
rlm@156: 
rlm@37: (defn triangles
rlm@37:   "Return a sequence of all the Triangles which compose a given
rlm@37:   Geometry." 
rlm@37:   [#^Geometry geom]
rlm@6:   (let
rlm@6:       [mesh (.getMesh geom)
rlm@6:        triangles (transient [])]
rlm@6:     (dorun
rlm@6:      (for [n (range (.getTriangleCount mesh))]
rlm@6:        (let [tri (Triangle.)]
rlm@6:          (.getTriangle mesh n tri)
rlm@37:         ;; (.calculateNormal tri)
rlm@37:         ;; (.calculateCenter tri)
rlm@6:          (conj! triangles tri))))
rlm@6:     (persistent! triangles)))
rlm@6:    
rlm@7: (defn get-ray-origin
rlm@37:   "Return the origin which a Ray would have to have to be in the exact
rlm@37:   center of a particular Triangle in the Geometry in World
rlm@37:   Coordinates."
rlm@7:   [geom tri]
rlm@7:   (let [new (Vector3f.)]
rlm@7:     (.calculateCenter tri)
rlm@37:     (.localToWorld geom (.getCenter tri) new) new))
rlm@6: 
rlm@7: (defn get-ray-direction
rlm@156:   "Return the direction which a Ray would have to have to be to point
rlm@37:   normal to the Triangle, in coordinates relative to the center of the
rlm@37:   Triangle."
rlm@7:   [geom tri]
rlm@9:   (let [n+c (Vector3f.)]
rlm@7:     (.calculateNormal tri)
rlm@9:     (.calculateCenter tri)
rlm@37:     (.localToWorld
rlm@37:      geom
rlm@37:      (.add (.getCenter tri) (.getNormal tri)) n+c)
rlm@37:     (.subtract n+c (get-ray-origin geom tri))))
rlm@37: 
rlm@156: ;; Every Mesh has many triangles, each with its own index.
rlm@156: ;; Every vertex has its own index as well.
rlm@37: 
rlm@156: (defn tactile-sensor-image
rlm@156:   "Return the touch-sensor distribution image in BufferedImage format,
rlm@156:    or nil if it does not exist."
rlm@156:   [#^Geometry obj]
rlm@156:   (if-let [image-path (meta-data obj "touch")]
rlm@156:     (ImageToAwt/convert
rlm@156:      (.getImage
rlm@156:       (.loadTexture
rlm@156:        (asset-manager)
rlm@156:        image-path))
rlm@156:     false false 0)))
rlm@156:      
rlm@156: 
rlm@156: 
rlm@156: (defn triangle
rlm@156:   "Get the triangle specified by triangle-index from the mesh within
rlm@156:   bounds."
rlm@156:   [#^Mesh mesh triangle-index]
rlm@156:   (let [scratch (Triangle.)]
rlm@156:     (.getTriangle mesh triangle-index scratch)
rlm@156:     scratch))
rlm@156: 
rlm@156: (defn triangle-vertex-indices
rlm@156:   "Get the triangle vertex indices of a given triangle from a given
rlm@156:    mesh."
rlm@156:   [#^Mesh mesh triangle-index]
rlm@156:   (let [indices (int-array 3)]
rlm@156:     (.getTriangle mesh triangle-index indices)
rlm@156:     (vec indices)))
rlm@156: 
rlm@156: (defn vertex-UV-coord
rlm@156:   "Get the uv-coordinates of the vertex named by vertex-index"
rlm@156:   [#^Mesh mesh vertex-index]
rlm@156:   (let [UV-buffer
rlm@156:         (.getData
rlm@156:          (.getBuffer
rlm@156:           mesh
rlm@156:           VertexBuffer$Type/TexCoord))]
rlm@156:     [(.get UV-buffer (* vertex-index 2))
rlm@156:      (.get UV-buffer (+ 1 (* vertex-index 2)))]))
rlm@156: 
rlm@156: (defn triangle-UV-coord
rlm@156:   "Get the uv-cooridnates of the triangle's verticies."
rlm@156:   [#^Mesh mesh width height triangle-index]
rlm@156:   (map (fn [[u v]] (vector (* width u) (* height v)))
rlm@156:        (map (partial vertex-UV-coord mesh)
rlm@156:             (triangle-vertex-indices mesh triangle-index))))
rlm@156:   
rlm@156: (defn same-side?
rlm@156:   "Given the points p1 and p2 and the reference point ref, is point p
rlm@156:   on the same side of the line that goes through p1 and p2 as ref is?" 
rlm@156:   [p1 p2 ref p]
rlm@156:   (<=
rlm@156:    0
rlm@156:    (.dot 
rlm@156:     (.cross (.subtract p2 p1) (.subtract p p1))
rlm@156:     (.cross (.subtract p2 p1) (.subtract ref p1)))))
rlm@156: 
rlm@156: (defn triangle-seq [#^Triangle tri]
rlm@156:   [(.get1 tri) (.get2 tri) (.get3 tri)])
rlm@156: 
rlm@156: (defn vector3f-seq [#^Vector3f v]
rlm@156:   [(.getX v) (.getY v) (.getZ v)])
rlm@156: 
rlm@156: (defn inside-triangle?
rlm@156:   "Is the point inside the triangle?"
rlm@156:   {:author "Dylan Holmes"}
rlm@156:   [#^Triangle tri #^Vector3f p]
rlm@156:   (let [[vert-1 vert-2 vert-3] (triangle-seq tri)]
rlm@156:     (and
rlm@156:      (same-side? vert-1 vert-2 vert-3 p)
rlm@156:      (same-side? vert-2 vert-3 vert-1 p)
rlm@156:      (same-side? vert-3 vert-1 vert-2 p))))
rlm@156: 
rlm@156: (defn triangle->matrix4f
rlm@156:   "Converts the triangle into a 4x4 matrix: The first three columns
rlm@156:    contain the vertices of the triangle; the last contains the unit
rlm@156:    normal of the triangle. The bottom row is filled with 1s."
rlm@156:   [#^Triangle t]
rlm@156:   (let [mat (Matrix4f.)
rlm@156:         [vert-1 vert-2 vert-3]
rlm@156:         ((comp vec map) #(.get t %) (range 3))
rlm@156:         unit-normal (do (.calculateNormal t)(.getNormal t))
rlm@156:         vertices [vert-1 vert-2 vert-3 unit-normal]]
rlm@156:     (dorun 
rlm@156:      (for [row (range 4) col (range 3)]
rlm@37:        (do
rlm@156:          (.set mat col row (.get (vertices row)col))
rlm@156:          (.set mat 3 row 1))))
rlm@156:     mat))
rlm@156: 
rlm@156: (defn triangle-transformation
rlm@156:   "Returns the affine transformation that converts each vertex in the
rlm@156:    first triangle into the corresponding vertex in the second
rlm@156:    triangle."
rlm@156:   [#^Triangle tri-1 #^Triangle tri-2]
rlm@156:   (.mult 
rlm@156:    (triangle->matrix4f tri-2)
rlm@156:    (.invert (triangle->matrix4f tri-1))))
rlm@156: 
rlm@156: (defn point->vector2f [[u v]]
rlm@156:   (Vector2f. u v))
rlm@156: 
rlm@156: (defn vector2f->vector3f [v]
rlm@156:   (Vector3f. (.getX v) (.getY v) 0))
rlm@156: 
rlm@156: (defn map-triangle [f #^Triangle tri]
rlm@156:   (Triangle.
rlm@156:    (f 0 (.get1 tri))
rlm@156:    (f 1 (.get2 tri))
rlm@156:    (f 2 (.get3 tri))))
rlm@156: 
rlm@156: (defn points->triangle
rlm@156:   "Convert a list of points into a triangle."
rlm@156:   [points]
rlm@156:   (apply #(Triangle. %1 %2 %3)
rlm@156:          (map (fn [point]
rlm@156:                 (let [point (vec point)]
rlm@156:                   (Vector3f. (get point 0 0)
rlm@156:                              (get point 1 0)
rlm@156:                              (get point 2 0))))
rlm@156:               (take 3 points))))
rlm@156: 
rlm@156: (defn convex-bounds
rlm@156:   ;;dylan
rlm@156:   "Returns the smallest square containing the given
rlm@156: vertices, as a vector of integers [left top width height]."
rlm@156:  ;; "Dimensions of the smallest integer bounding square of the list of
rlm@156:  ;;  2D verticies in the form: [x y width height]."
rlm@156:   [uv-verts]
rlm@156:   (let [xs (map first uv-verts)
rlm@156:         ys (map second uv-verts)
rlm@156:         x0 (Math/floor (apply min xs))
rlm@156:         y0 (Math/floor (apply min ys))
rlm@156:         x1 (Math/ceil (apply max xs))
rlm@156:         y1 (Math/ceil (apply max ys))]
rlm@156:     [x0 y0 (- x1 x0) (- y1 y0)]))
rlm@156: 
rlm@156: (defn sensors-in-triangle
rlm@156:   ;;dylan
rlm@156:   "Locate the touch sensors in the triangle, returning a map of their UV and geometry-relative coordinates."
rlm@156:   ;;"Find the locations of the touch sensors within a triangle in both
rlm@156:   ;; UV and gemoetry relative coordinates."
rlm@156:   [image mesh tri-index]
rlm@156:   (let [width (.getWidth image)
rlm@156:         height (.getHeight image)
rlm@156:         UV-vertex-coords (triangle-UV-coord mesh width height tri-index)
rlm@156:         bounds (convex-bounds UV-vertex-coords)
rlm@156:         
rlm@156:         cutout-triangle (points->triangle UV-vertex-coords)
rlm@156:         UV-sensor-coords
rlm@156:         (filter (comp (partial inside-triangle? cutout-triangle)
rlm@156:                       (fn [[u v]] (Vector3f. u v 0)))
rlm@156:                 (white-coordinates image bounds))
rlm@156:         UV->geometry (triangle-transformation
rlm@156:                       cutout-triangle
rlm@156:                       (triangle mesh tri-index))
rlm@156:         geometry-sensor-coords
rlm@156:         (map (fn [[u v]] (.mult UV->geometry (Vector3f. u v 0)))
rlm@156:              UV-sensor-coords)]
rlm@156:   {:UV UV-sensor-coords :geometry geometry-sensor-coords}))
rlm@156: 
rlm@156: (defn-memo locate-feelers
rlm@156:   "Search the geometry's tactile UV image for touch sensors, returning
rlm@156:   their positions in geometry-relative coordinates."
rlm@156:   [#^Geometry geo]
rlm@156:   (let [mesh (.getMesh geo)
rlm@156:         num-triangles (.getTriangleCount mesh)]
rlm@156:     (if-let [image (tactile-sensor-image geo)]
rlm@156:       (map
rlm@156:        (partial sensors-in-triangle image mesh)
rlm@156:        (range num-triangles))
rlm@156:       (repeat (.getTriangleCount mesh) {:UV nil :geometry nil}))))
rlm@156: 
rlm@156: 
rlm@156: 
rlm@156: (defn-memo touch-topology [#^Gemoetry geo]
rlm@156:   (vec (collapse (reduce concat (map :UV (locate-feelers geo))))))
rlm@156: 
rlm@156: (defn-memo feeler-coordinates [#^Geometry geo]
rlm@156:   (vec (map :geometry (locate-feelers geo))))
rlm@156: 
rlm@156: (defn enable-touch [#^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@156:   
rlm@156: (defn touch [#^Node pieces]
rlm@156:   (filter (comp not nil?)
rlm@156:           (map enable-touch
rlm@156:                (filter #(isa? (class %) Geometry)
rlm@156:                        (node-seq pieces)))))
rlm@156: 
rlm@156: 
rlm@37: #+end_src
rlm@37: 
rlm@37: 
rlm@37: * Example
rlm@37: 
rlm@66: #+name: touch-test
rlm@37: #+begin_src clojure 
rlm@68: (ns cortex.test.touch
rlm@59:   (:use (cortex world util touch))
rlm@59:   (:import
rlm@59:    com.jme3.scene.shape.Sphere
rlm@59:    com.jme3.math.ColorRGBA
rlm@59:    com.jme3.math.Vector3f
rlm@59:    com.jme3.material.RenderState$BlendMode
rlm@59:    com.jme3.renderer.queue.RenderQueue$Bucket
rlm@59:    com.jme3.scene.shape.Box
rlm@68:    com.jme3.scene.Node))
rlm@39: 
rlm@7: (defn ray-origin-debug
rlm@9:   [ray color]
rlm@7:   (make-shape
rlm@20:    (assoc base-shape
rlm@20:      :shape (Sphere. 5 5 0.05)
rlm@20:      :name "arrow"
rlm@20:      :color color
rlm@20:      :texture false
rlm@20:      :physical? false
rlm@20:      :position
rlm@20:      (.getOrigin ray))))
rlm@6: 
rlm@9: (defn ray-debug [ray color]
rlm@6:   (make-shape
rlm@6:    (assoc
rlm@6:        base-shape
rlm@6:      :name "debug-ray"
rlm@6:      :physical? false
rlm@6:      :shape (com.jme3.scene.shape.Line.
rlm@6:              (.getOrigin ray)
rlm@6:              (.add
rlm@6:               (.getOrigin ray)
rlm@6:               (.mult (.getDirection ray)
rlm@6:                      (float (.getLimit ray))))))))
rlm@6:              
rlm@6: 
rlm@10: (defn contact-color [contacts]
rlm@10:   (case contacts
rlm@10:     0 ColorRGBA/Gray
rlm@37:     1  ColorRGBA/Red 
rlm@10:     2  ColorRGBA/Green 
rlm@10:     3  ColorRGBA/Yellow 
rlm@10:     4  ColorRGBA/Orange 
rlm@10:     5  ColorRGBA/Red 
rlm@10:     6  ColorRGBA/Magenta 
rlm@10:     7  ColorRGBA/Pink 
rlm@10:     8  ColorRGBA/White))
rlm@6: 
rlm@14: (defn update-ray-debug [node ray contacts]
rlm@14:   (let [origin (.getChild node 0)]
rlm@14:     (.setLocalTranslation origin (.getOrigin ray))
rlm@14:     (.setColor (.getMaterial origin) "Color" (contact-color contacts))))
rlm@14: 
rlm@13: (defn init-node
rlm@13:   [debug-node rays]
rlm@12:     (.detachAllChildren debug-node)
rlm@13:     (dorun 
rlm@13:      (for [ray rays]
rlm@13:        (do
rlm@13:          (.attachChild
rlm@13:           debug-node 
rlm@13:           (doto (Node.)
rlm@14:             (.attachChild (ray-origin-debug ray ColorRGBA/Gray))
rlm@20:             (.attachChild (ray-debug ray ColorRGBA/Gray))
rlm@14:             ))))))
rlm@14: 
rlm@13: (defn manage-ray-debug-node [debug-node geom touch-data limit]
rlm@13:   (let [rays (normal-rays limit geom)]
rlm@13:     (if (not= (count (.getChildren debug-node)) (count touch-data))
rlm@13:       (init-node debug-node rays))
rlm@13:     (dorun 
rlm@13:      (for [n (range (count touch-data))]
rlm@14:        (update-ray-debug
rlm@14:         (.getChild debug-node n) (nth rays n) (nth touch-data n))))))
rlm@12: 
rlm@0: (defn transparent-sphere []
rlm@0:   (doto
rlm@0:       (make-shape
rlm@0:        (merge base-shape
rlm@0: 	      {:position (Vector3f. 0 2 0)
rlm@0: 	       :name "the blob."
rlm@0: 	       :material "Common/MatDefs/Misc/Unshaded.j3md"
rlm@0: 	       :texture "Textures/purpleWisp.png"
rlm@0: 	       :physical? true
rlm@0: 	       :mass 70
rlm@0: 	       :color ColorRGBA/Blue
rlm@0: 	       :shape (Sphere. 10 10 1)}))
rlm@0:     (-> (.getMaterial)
rlm@0: 	(.getAdditionalRenderState)
rlm@0: 	(.setBlendMode RenderState$BlendMode/Alpha))
rlm@0:     (.setQueueBucket RenderQueue$Bucket/Transparent)))
rlm@0: 
rlm@0: (defn transparent-box []
rlm@0:     (doto
rlm@0:       (make-shape
rlm@0:        (merge base-shape
rlm@0: 	      {:position (Vector3f. 0 2 0)
rlm@10: 	       :name "box"
rlm@0: 	       :material "Common/MatDefs/Misc/Unshaded.j3md"
rlm@0: 	       :texture "Textures/purpleWisp.png"
rlm@0: 	       :physical? true
rlm@0: 	       :mass 70
rlm@0: 	       :color ColorRGBA/Blue
rlm@0: 	       :shape (Box. 1 1 1)}))
rlm@0:     (-> (.getMaterial)
rlm@0: 	(.getAdditionalRenderState)
rlm@0: 	(.setBlendMode RenderState$BlendMode/Alpha))
rlm@0:     (.setQueueBucket RenderQueue$Bucket/Transparent)))
rlm@0: 
rlm@6: (defn transparent-floor []
rlm@6:   (doto
rlm@6:       (box 5 0.2 5  :mass 0 :position (Vector3f. 0 -2 0)
rlm@6:            :material "Common/MatDefs/Misc/Unshaded.j3md"
rlm@10:            :texture "Textures/redWisp.png"
rlm@10:            :name "floor")
rlm@6:     (-> (.getMaterial)
rlm@6:         (.getAdditionalRenderState)
rlm@6:         (.setBlendMode RenderState$BlendMode/Alpha))
rlm@6:     (.setQueueBucket RenderQueue$Bucket/Transparent)))
rlm@6: 
rlm@69: (defn test-skin 
rlm@69:   "Testing touch:
rlm@69:    you should see a ball which responds to the table
rlm@69:    and whatever balls hit it."
rlm@69:   []
rlm@0:   (let [b
rlm@58:         ;;(transparent-box)
rlm@58:         (transparent-sphere)
rlm@10:         ;;(sphere)
rlm@58:         f (transparent-floor)
rlm@6:         debug-node (Node.)
rlm@12:         node      (doto (Node.) (.attachChild b) (.attachChild f))
rlm@12:         root-node (doto (Node.) (.attachChild node)
rlm@12:                         (.attachChild debug-node))
rlm@12:         ]
rlm@0:     
rlm@0:     (world
rlm@12:      root-node
rlm@15:      {"key-return" (fire-cannon-ball node)}
rlm@0:      (fn [world]
rlm@20:        ;;  (Capture/SimpleCaptureVideo
rlm@20:        ;;   world 
rlm@20:        ;;   (file-str "/home/r/proj/cortex/tmp/blob.avi"))
rlm@20:        ;;  (no-logging)
rlm@20:        ;;(enable-debug world)
rlm@20:        ;;  (set-accuracy world (/ 1 60))
rlm@58:        )
rlm@58:      
rlm@0:      (fn [& _]
rlm@19:        (let [sensitivity 0.2
rlm@18:              touch-data (touch-percieve sensitivity b node)]
rlm@68:          (manage-ray-debug-node debug-node b touch-data sensitivity))
rlm@0:        ))))
rlm@0: 
rlm@37: 
rlm@0: #+end_src
rlm@0: 
rlm@0: 
rlm@10: 
rlm@10: 
rlm@6:   
rlm@0: * 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: <<touch-test>>
rlm@39: #+end_src
rlm@39: 
rlm@0: 
rlm@0: 
rlm@0:   
rlm@32: 
rlm@32: