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@37: is to thousands of small /touch receptors/ along the geometries which
rlm@37: make up the creature's body.  The number of touch receptors in a given
rlm@37: area is determined by how complicated that area is, as determined by
rlm@37: the total number of triangles in that region. This way, complicated
rlm@37: regions like the hands/face, etc. get more touch receptors than
rlm@37: simpler areas of the body.
rlm@0: 
rlm@0: #+srcname: 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@37:   (:use (cortex world util))
rlm@37:   (:import com.jme3.scene.Geometry)
rlm@39:   (:import com.jme3.collision.CollisionResults)
rlm@39:   (:import (com.jme3.math Triangle Vector3f Ray)))
rlm@37:    
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@37:   "Return the direction which a Ray would have to have to be in the
rlm@37:   exact center of a particular Triangle in the Geometry, pointing
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@37: (defn normal-rays
rlm@37:   "For each Triangle which comprises the Geometry, returns a Ray which
rlm@37:   is centered on that Triangle, points outward in a normal direction,
rlm@37:   and extends for =limit= distance."
rlm@37:   [limit #^Geometry geom]
rlm@37:   (vec
rlm@37:    (map 
rlm@37:     (fn [tri]
rlm@37:       (doto 
rlm@37:           (Ray. (get-ray-origin geom tri)
rlm@37:                 (get-ray-direction geom tri))
rlm@37:         (.setLimit limit)))
rlm@37:     (triangles geom))))
rlm@37: 
rlm@37: (defn touch-percieve
rlm@37:   "Augment a Geometry with the sense of touch. Returns a sequence of
rlm@37:   non-negative integers, one for each triangle, with the value of the
rlm@37:   integer describing how many objects a ray of length =limit=, normal
rlm@37:   to the triangle and originating from its center, encountered. The
rlm@37:   Geometry itself is not counted among the results."
rlm@37:   [limit geom node]
rlm@37:   (let [normals  (normal-rays limit geom)]
rlm@37:     (doall
rlm@37:      (for [ray normals]
rlm@37:        (do
rlm@37:          (let [results (CollisionResults.)]
rlm@37:            (.collideWith node ray results)
rlm@37:            (let [touch-objects
rlm@37:                  (set (filter #(not (= geom %))
rlm@37:                               (map #(.getGeometry %) results)))]
rlm@37:              (count touch-objects))))))))
rlm@37: #+end_src
rlm@37: 
rlm@37: 
rlm@37: * Example
rlm@37: 
rlm@39: #+srcname: touch-test
rlm@37: #+begin_src clojure 
rlm@39: (ns test.touch
rlm@39:   (:use (cortex world util touch)))
rlm@37: 
rlm@39: (cortex.import/mega-import-jme3)
rlm@7:   
rlm@39: (import java.util.logging.Level)
rlm@39: (import java.util.logging.Logger)
rlm@39: (use 'hello.brick-wall)
rlm@39: 
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@13:     (println-repl "Init touch debug node.")
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@12: 
rlm@6: 
rlm@11: (defn no-logging []
rlm@11:   (.setLevel (Logger/getLogger "com.jme3") Level/OFF))
rlm@11: 
rlm@11: (defn set-accuracy [world new-accuracy]
rlm@11:   (let [physics-manager (.getState (.getStateManager world) BulletAppState)]
rlm@11:     (.setAccuracy (.getPhysicsSpace physics-manager) (float new-accuracy))))
rlm@11: 
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@0: (defn test-skin []
rlm@0:   (let [b
rlm@18: 	;;(transparent-box)
rlm@18: 	(transparent-sphere)
rlm@10:         ;;(sphere)
rlm@6: 	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@11:      )
rlm@0: 
rlm@0:      (fn [& _]
rlm@19:        (let [sensitivity 0.2
rlm@18:              touch-data (touch-percieve sensitivity b node)]
rlm@18:          (manage-ray-debug-node debug-node b touch-data sensitivity)
rlm@18:          )
rlm@11:        (Thread/sleep 10)
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@39: #+begin_src clojure :tangle ../src/test/touch.clj 
rlm@39: <<touch-test>>
rlm@39: #+end_src
rlm@39: 
rlm@0: 
rlm@0: 
rlm@0:   
rlm@32: 
rlm@32: