Mercurial > cortex
view org/touch.org @ 219:5f14fd7b1288
minor corrections from reviewing with dad
author | Robert McIntyre <rlm@mit.edu> |
---|---|
date | Sat, 11 Feb 2012 00:51:54 -0700 |
parents | facc2ef3fe5c |
children | e5db1d2ff9a8 |
line wrap: on
line source
1 #+title: Simulated Sense of Touch2 #+author: Robert McIntyre3 #+email: rlm@mit.edu4 #+description: Simulated touch for AI research using JMonkeyEngine and clojure.5 #+keywords: simulation, tactile sense, jMonkeyEngine3, clojure6 #+SETUPFILE: ../../aurellem/org/setup.org7 #+INCLUDE: ../../aurellem/org/level-0.org9 * Touch11 My creatures need to be able to feel their environments. The idea here12 is to create thousands of small /touch receptors/ along the geometries13 which make up the creature's body. The number of touch receptors in a14 given area is determined by how complicated that area is, as15 determined by the total number of triangles in that region. This way,16 complicated regions like the hands/face, etc. get more touch receptors17 than simpler areas of the body.19 #+name: skin-main20 #+begin_src clojure21 (ns cortex.touch22 "Simulate the sense of touch in jMonkeyEngine3. Enables any Geometry23 to be outfitted with touch sensors with density proportional to the24 density of triangles along the surface of the Geometry. Enables a25 Geometry to know what parts of itself are touching nearby objects."26 {:author "Robert McIntyre"}27 (:use (cortex world util sense))28 (:use clojure.contrib.def)29 (:import (com.jme3.scene Geometry Node Mesh))30 (:import com.jme3.collision.CollisionResults)31 (:import com.jme3.scene.VertexBuffer$Type)32 (:import (com.jme3.math Triangle Vector3f Vector2f Ray Matrix4f)))34 (defn triangles35 "Return a sequence of all the Triangles which compose a given36 Geometry."37 [#^Geometry geom]38 (let39 [mesh (.getMesh geom)40 triangles (transient [])]41 (dorun42 (for [n (range (.getTriangleCount mesh))]43 (let [tri (Triangle.)]44 (.getTriangle mesh n tri)45 ;; (.calculateNormal tri)46 ;; (.calculateCenter tri)47 (conj! triangles tri))))48 (persistent! triangles)))50 (defn get-ray-origin51 "Return the origin which a Ray would have to have to be in the exact52 center of a particular Triangle in the Geometry in World53 Coordinates."54 [geom tri]55 (let [new (Vector3f.)]56 (.calculateCenter tri)57 (.localToWorld geom (.getCenter tri) new) new))59 (defn get-ray-direction60 "Return the direction which a Ray would have to have to be to point61 normal to the Triangle, in coordinates relative to the center of the62 Triangle."63 [geom tri]64 (let [n+c (Vector3f.)]65 (.calculateNormal tri)66 (.calculateCenter tri)67 (.localToWorld68 geom69 (.add (.getCenter tri) (.getNormal tri)) n+c)70 (.subtract n+c (get-ray-origin geom tri))))72 ;; Every Mesh has many triangles, each with its own index.73 ;; Every vertex has its own index as well.75 (defn tactile-sensor-profile76 "Return the touch-sensor distribution image in BufferedImage format,77 or nil if it does not exist."78 [#^Geometry obj]79 (if-let [image-path (meta-data obj "touch")]80 (load-image image-path)))82 (defn triangle83 "Get the triangle specified by triangle-index from the mesh within84 bounds."85 [#^Mesh mesh triangle-index]86 (let [scratch (Triangle.)]87 (.getTriangle mesh triangle-index scratch)88 scratch))90 (defn triangle-vertex-indices91 "Get the triangle vertex indices of a given triangle from a given92 mesh."93 [#^Mesh mesh triangle-index]94 (let [indices (int-array 3)]95 (.getTriangle mesh triangle-index indices)96 (vec indices)))98 (defn vertex-UV-coord99 "Get the uv-coordinates of the vertex named by vertex-index"100 [#^Mesh mesh vertex-index]101 (let [UV-buffer102 (.getData103 (.getBuffer104 mesh105 VertexBuffer$Type/TexCoord))]106 [(.get UV-buffer (* vertex-index 2))107 (.get UV-buffer (+ 1 (* vertex-index 2)))]))109 (defn triangle-UV-coord110 "Get the uv-cooridnates of the triangle's verticies."111 [#^Mesh mesh width height triangle-index]112 (map (fn [[u v]] (vector (* width u) (* height v)))113 (map (partial vertex-UV-coord mesh)114 (triangle-vertex-indices mesh triangle-index))))116 (defn same-side?117 "Given the points p1 and p2 and the reference point ref, is point p118 on the same side of the line that goes through p1 and p2 as ref is?"119 [p1 p2 ref p]120 (<=121 0122 (.dot123 (.cross (.subtract p2 p1) (.subtract p p1))124 (.cross (.subtract p2 p1) (.subtract ref p1)))))126 (defn triangle-seq [#^Triangle tri]127 [(.get1 tri) (.get2 tri) (.get3 tri)])129 (defn vector3f-seq [#^Vector3f v]130 [(.getX v) (.getY v) (.getZ v)])132 (defn inside-triangle?133 "Is the point inside the triangle?"134 {:author "Dylan Holmes"}135 [#^Triangle tri #^Vector3f p]136 (let [[vert-1 vert-2 vert-3] (triangle-seq tri)]137 (and138 (same-side? vert-1 vert-2 vert-3 p)139 (same-side? vert-2 vert-3 vert-1 p)140 (same-side? vert-3 vert-1 vert-2 p))))142 (defn triangle->matrix4f143 "Converts the triangle into a 4x4 matrix: The first three columns144 contain the vertices of the triangle; the last contains the unit145 normal of the triangle. The bottom row is filled with 1s."146 [#^Triangle t]147 (let [mat (Matrix4f.)148 [vert-1 vert-2 vert-3]149 ((comp vec map) #(.get t %) (range 3))150 unit-normal (do (.calculateNormal t)(.getNormal t))151 vertices [vert-1 vert-2 vert-3 unit-normal]]152 (dorun153 (for [row (range 4) col (range 3)]154 (do155 (.set mat col row (.get (vertices row)col))156 (.set mat 3 row 1))))157 mat))159 (defn triangle-transformation160 "Returns the affine transformation that converts each vertex in the161 first triangle into the corresponding vertex in the second162 triangle."163 [#^Triangle tri-1 #^Triangle tri-2]164 (.mult165 (triangle->matrix4f tri-2)166 (.invert (triangle->matrix4f tri-1))))168 (defn point->vector2f [[u v]]169 (Vector2f. u v))171 (defn vector2f->vector3f [v]172 (Vector3f. (.getX v) (.getY v) 0))174 (defn map-triangle [f #^Triangle tri]175 (Triangle.176 (f 0 (.get1 tri))177 (f 1 (.get2 tri))178 (f 2 (.get3 tri))))180 (defn points->triangle181 "Convert a list of points into a triangle."182 [points]183 (apply #(Triangle. %1 %2 %3)184 (map (fn [point]185 (let [point (vec point)]186 (Vector3f. (get point 0 0)187 (get point 1 0)188 (get point 2 0))))189 (take 3 points))))191 (defn convex-bounds192 "Returns the smallest square containing the given vertices, as a193 vector of integers [left top width height]."194 [uv-verts]195 (let [xs (map first uv-verts)196 ys (map second uv-verts)197 x0 (Math/floor (apply min xs))198 y0 (Math/floor (apply min ys))199 x1 (Math/ceil (apply max xs))200 y1 (Math/ceil (apply max ys))]201 [x0 y0 (- x1 x0) (- y1 y0)]))203 (defn sensors-in-triangle204 "Locate the touch sensors in the triangle, returning a map of their205 UV and geometry-relative coordinates."206 [image mesh tri-index]207 (let [width (.getWidth image)208 height (.getHeight image)209 UV-vertex-coords (triangle-UV-coord mesh width height tri-index)210 bounds (convex-bounds UV-vertex-coords)212 cutout-triangle (points->triangle UV-vertex-coords)213 UV-sensor-coords214 (filter (comp (partial inside-triangle? cutout-triangle)215 (fn [[u v]] (Vector3f. u v 0)))216 (white-coordinates image bounds))217 UV->geometry (triangle-transformation218 cutout-triangle219 (triangle mesh tri-index))220 geometry-sensor-coords221 (map (fn [[u v]] (.mult UV->geometry (Vector3f. u v 0)))222 UV-sensor-coords)]223 {:UV UV-sensor-coords :geometry geometry-sensor-coords}))225 (defn-memo locate-feelers226 "Search the geometry's tactile UV profile for touch sensors,227 returning their positions in geometry-relative coordinates."228 [#^Geometry geo]229 (let [mesh (.getMesh geo)230 num-triangles (.getTriangleCount mesh)]231 (if-let [image (tactile-sensor-profile geo)]232 (map233 (partial sensors-in-triangle image mesh)234 (range num-triangles))235 (repeat (.getTriangleCount mesh) {:UV nil :geometry nil}))))237 (defn-memo touch-topology238 "Return a sequence of vectors of the form [x y] describing the239 \"topology\" of the tactile sensors. Points that are close together240 in the touch-topology are generally close together in the simulation."241 [#^Gemoetry geo]242 (vec (collapse (reduce concat (map :UV (locate-feelers geo))))))244 (defn-memo feeler-coordinates245 "The location of the touch sensors in world-space coordinates."246 [#^Geometry geo]247 (vec (map :geometry (locate-feelers geo))))249 (defn touch-fn250 "Returns a function which returns tactile sensory data when called251 inside a running simulation."252 [#^Geometry geo]253 (let [feeler-coords (feeler-coordinates geo)254 tris (triangles geo)255 limit 0.1256 ;;results (CollisionResults.)257 ]258 (if (empty? (touch-topology geo))259 nil260 (fn [node]261 (let [sensor-origins262 (map263 #(map (partial local-to-world geo) %)264 feeler-coords)265 triangle-normals266 (map (partial get-ray-direction geo)267 tris)268 rays269 (flatten270 (map (fn [origins norm]271 (map #(doto (Ray. % norm)272 (.setLimit limit)) origins))273 sensor-origins triangle-normals))]274 (vector275 (touch-topology geo)276 (vec277 (for [ray rays]278 (do279 (let [results (CollisionResults.)]280 (.collideWith node ray results)281 (let [touch-objects282 (filter #(not (= geo (.getGeometry %)))283 results)]284 (- 255285 (if (empty? touch-objects) 255286 (rem287 (int288 (* 255 (/ (.getDistance289 (first touch-objects)) limit)))290 256))))))))))))))292 (defn touch!293 "Endow the creature with the sense of touch. Returns a sequence of294 functions, one for each body part with a tactile-sensor-proile,295 each of which when called returns sensory data for that body part."296 [#^Node creature]297 (filter298 (comp not nil?)299 (map touch-fn300 (filter #(isa? (class %) Geometry)301 (node-seq creature)))))303 (defn view-touch304 "Creates a function which accepts a list of touch sensor-data and305 displays each element to the screen."306 []307 (view-sense308 (fn309 [[coords sensor-data]]310 (let [image (points->image coords)]311 (dorun312 (for [i (range (count coords))]313 (.setRGB image ((coords i) 0) ((coords i) 1)314 (gray (sensor-data i)))))315 image))))318 #+end_src321 * Example323 #+name: touch-test324 #+begin_src clojure325 (ns cortex.test.touch326 (:use (cortex world util touch))327 (:import328 com.jme3.scene.shape.Sphere329 com.jme3.math.ColorRGBA330 com.jme3.math.Vector3f331 com.jme3.material.RenderState$BlendMode332 com.jme3.renderer.queue.RenderQueue$Bucket333 com.jme3.scene.shape.Box334 com.jme3.scene.Node))336 (defn ray-origin-debug337 [ray color]338 (make-shape339 (assoc base-shape340 :shape (Sphere. 5 5 0.05)341 :name "arrow"342 :color color343 :texture false344 :physical? false345 :position346 (.getOrigin ray))))348 (defn ray-debug [ray color]349 (make-shape350 (assoc351 base-shape352 :name "debug-ray"353 :physical? false354 :shape (com.jme3.scene.shape.Line.355 (.getOrigin ray)356 (.add357 (.getOrigin ray)358 (.mult (.getDirection ray)359 (float (.getLimit ray))))))))362 (defn contact-color [contacts]363 (case contacts364 0 ColorRGBA/Gray365 1 ColorRGBA/Red366 2 ColorRGBA/Green367 3 ColorRGBA/Yellow368 4 ColorRGBA/Orange369 5 ColorRGBA/Red370 6 ColorRGBA/Magenta371 7 ColorRGBA/Pink372 8 ColorRGBA/White))374 (defn update-ray-debug [node ray contacts]375 (let [origin (.getChild node 0)]376 (.setLocalTranslation origin (.getOrigin ray))377 (.setColor (.getMaterial origin) "Color" (contact-color contacts))))379 (defn init-node380 [debug-node rays]381 (.detachAllChildren debug-node)382 (dorun383 (for [ray rays]384 (do385 (.attachChild386 debug-node387 (doto (Node.)388 (.attachChild (ray-origin-debug ray ColorRGBA/Gray))389 (.attachChild (ray-debug ray ColorRGBA/Gray))390 ))))))392 (defn manage-ray-debug-node [debug-node geom touch-data limit]393 (let [rays (normal-rays limit geom)]394 (if (not= (count (.getChildren debug-node)) (count touch-data))395 (init-node debug-node rays))396 (dorun397 (for [n (range (count touch-data))]398 (update-ray-debug399 (.getChild debug-node n) (nth rays n) (nth touch-data n))))))401 (defn transparent-sphere []402 (doto403 (make-shape404 (merge base-shape405 {:position (Vector3f. 0 2 0)406 :name "the blob."407 :material "Common/MatDefs/Misc/Unshaded.j3md"408 :texture "Textures/purpleWisp.png"409 :physical? true410 :mass 70411 :color ColorRGBA/Blue412 :shape (Sphere. 10 10 1)}))413 (-> (.getMaterial)414 (.getAdditionalRenderState)415 (.setBlendMode RenderState$BlendMode/Alpha))416 (.setQueueBucket RenderQueue$Bucket/Transparent)))418 (defn transparent-box []419 (doto420 (make-shape421 (merge base-shape422 {:position (Vector3f. 0 2 0)423 :name "box"424 :material "Common/MatDefs/Misc/Unshaded.j3md"425 :texture "Textures/purpleWisp.png"426 :physical? true427 :mass 70428 :color ColorRGBA/Blue429 :shape (Box. 1 1 1)}))430 (-> (.getMaterial)431 (.getAdditionalRenderState)432 (.setBlendMode RenderState$BlendMode/Alpha))433 (.setQueueBucket RenderQueue$Bucket/Transparent)))435 (defn transparent-floor []436 (doto437 (box 5 0.2 5 :mass 0 :position (Vector3f. 0 -2 0)438 :material "Common/MatDefs/Misc/Unshaded.j3md"439 :texture "Textures/redWisp.png"440 :name "floor")441 (-> (.getMaterial)442 (.getAdditionalRenderState)443 (.setBlendMode RenderState$BlendMode/Alpha))444 (.setQueueBucket RenderQueue$Bucket/Transparent)))446 (defn test-skin447 "Testing touch:448 you should see a ball which responds to the table449 and whatever balls hit it."450 []451 (let [b452 ;;(transparent-box)453 (transparent-sphere)454 ;;(sphere)455 f (transparent-floor)456 debug-node (Node.)457 node (doto (Node.) (.attachChild b) (.attachChild f))458 root-node (doto (Node.) (.attachChild node)459 (.attachChild debug-node))460 ]462 (world463 root-node464 {"key-return" (fire-cannon-ball node)}465 (fn [world]466 ;; (Capture/SimpleCaptureVideo467 ;; world468 ;; (file-str "/home/r/proj/cortex/tmp/blob.avi"))469 ;; (no-logging)470 ;;(enable-debug world)471 ;; (set-accuracy world (/ 1 60))472 )474 (fn [& _]475 (let [sensitivity 0.2476 touch-data (touch-percieve sensitivity b node)]477 (manage-ray-debug-node debug-node b touch-data sensitivity))478 ))))481 #+end_src487 * COMMENT code generation488 #+begin_src clojure :tangle ../src/cortex/touch.clj489 <<skin-main>>490 #+end_src492 #+begin_src clojure :tangle ../src/cortex/test/touch.clj493 <<touch-test>>494 #+end_src