Mercurial > cortex

annotate org/touch.org @ 179:11bd5f0625ad

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
organized imports in touch
author Robert McIntyre <rlm@mit.edu>
date Sat, 04 Feb 2012 07:09:40 -0700
parents 6fba17a74a57
children cfb71209ddc6
rev   line source
rlm@37 1 #+title: Simulated Sense of Touch
rlm@0 2 #+author: Robert McIntyre
rlm@0 3 #+email: rlm@mit.edu
rlm@37 4 #+description: Simulated touch for AI research using JMonkeyEngine and clojure.
rlm@37 5 #+keywords: simulation, tactile sense, jMonkeyEngine3, clojure
rlm@4 6 #+SETUPFILE: ../../aurellem/org/setup.org
rlm@4 7 #+INCLUDE: ../../aurellem/org/level-0.org
rlm@0 8
rlm@37 9 * Touch
rlm@0 10
rlm@37 11 My creatures need to be able to feel their environments. The idea here
rlm@43 12 is to create thousands of small /touch receptors/ along the geometries
rlm@43 13 which make up the creature's body. The number of touch receptors in a
rlm@43 14 given area is determined by how complicated that area is, as
rlm@43 15 determined by the total number of triangles in that region. This way,
rlm@43 16 complicated regions like the hands/face, etc. get more touch receptors
rlm@43 17 than simpler areas of the body.
rlm@0 18
rlm@66 19 #+name: skin-main
rlm@0 20 #+begin_src clojure
rlm@37 21 (ns cortex.touch
rlm@37 22 "Simulate the sense of touch in jMonkeyEngine3. Enables any Geometry
rlm@37 23 to be outfitted with touch sensors with density proportional to the
rlm@37 24 density of triangles along the surface of the Geometry. Enables a
rlm@37 25 Geometry to know what parts of itself are touching nearby objects."
rlm@37 26 {:author "Robert McIntyre"}
rlm@156 27 (:use (cortex world util sense))
rlm@178 28 (:use clojure.contrib.def)
rlm@179 29 (:import (com.jme3.scene Geometry Node Mesh))
rlm@39 30 (:import com.jme3.collision.CollisionResults)
rlm@179 31 (:import com.jme3.scene.VertexBuffer$Type)
rlm@179 32 (:import (com.jme3.math Triangle Vector3f Vector2f Ray Matrix4f)))
rlm@37 33
rlm@37 34 (defn triangles
rlm@37 35 "Return a sequence of all the Triangles which compose a given
rlm@37 36 Geometry."
rlm@37 37 [#^Geometry geom]
rlm@6 38 (let
rlm@6 39 [mesh (.getMesh geom)
rlm@6 40 triangles (transient [])]
rlm@6 41 (dorun
rlm@6 42 (for [n (range (.getTriangleCount mesh))]
rlm@6 43 (let [tri (Triangle.)]
rlm@6 44 (.getTriangle mesh n tri)
rlm@37 45 ;; (.calculateNormal tri)
rlm@37 46 ;; (.calculateCenter tri)
rlm@6 47 (conj! triangles tri))))
rlm@6 48 (persistent! triangles)))
rlm@6 49
rlm@7 50 (defn get-ray-origin
rlm@37 51 "Return the origin which a Ray would have to have to be in the exact
rlm@37 52 center of a particular Triangle in the Geometry in World
rlm@37 53 Coordinates."
rlm@7 54 [geom tri]
rlm@7 55 (let [new (Vector3f.)]
rlm@7 56 (.calculateCenter tri)
rlm@37 57 (.localToWorld geom (.getCenter tri) new) new))
rlm@6 58
rlm@7 59 (defn get-ray-direction
rlm@156 60 "Return the direction which a Ray would have to have to be to point
rlm@37 61 normal to the Triangle, in coordinates relative to the center of the
rlm@37 62 Triangle."
rlm@7 63 [geom tri]
rlm@9 64 (let [n+c (Vector3f.)]
rlm@7 65 (.calculateNormal tri)
rlm@9 66 (.calculateCenter tri)
rlm@37 67 (.localToWorld
rlm@37 68 geom
rlm@37 69 (.add (.getCenter tri) (.getNormal tri)) n+c)
rlm@37 70 (.subtract n+c (get-ray-origin geom tri))))
rlm@37 71
rlm@156 72 ;; Every Mesh has many triangles, each with its own index.
rlm@156 73 ;; Every vertex has its own index as well.
rlm@37 74
rlm@178 75 (defn tactile-sensor-profile
rlm@156 76 "Return the touch-sensor distribution image in BufferedImage format,
rlm@156 77 or nil if it does not exist."
rlm@156 78 [#^Geometry obj]
rlm@156 79 (if-let [image-path (meta-data obj "touch")]
rlm@178 80 (load-image image-path)))
rlm@156 81
rlm@156 82 (defn triangle
rlm@156 83 "Get the triangle specified by triangle-index from the mesh within
rlm@156 84 bounds."
rlm@156 85 [#^Mesh mesh triangle-index]
rlm@156 86 (let [scratch (Triangle.)]
rlm@156 87 (.getTriangle mesh triangle-index scratch)
rlm@156 88 scratch))
rlm@156 89
rlm@156 90 (defn triangle-vertex-indices
rlm@156 91 "Get the triangle vertex indices of a given triangle from a given
rlm@156 92 mesh."
rlm@156 93 [#^Mesh mesh triangle-index]
rlm@156 94 (let [indices (int-array 3)]
rlm@156 95 (.getTriangle mesh triangle-index indices)
rlm@156 96 (vec indices)))
rlm@156 97
rlm@156 98 (defn vertex-UV-coord
rlm@156 99 "Get the uv-coordinates of the vertex named by vertex-index"
rlm@156 100 [#^Mesh mesh vertex-index]
rlm@156 101 (let [UV-buffer
rlm@156 102 (.getData
rlm@156 103 (.getBuffer
rlm@156 104 mesh
rlm@156 105 VertexBuffer$Type/TexCoord))]
rlm@156 106 [(.get UV-buffer (* vertex-index 2))
rlm@156 107 (.get UV-buffer (+ 1 (* vertex-index 2)))]))
rlm@156 108
rlm@156 109 (defn triangle-UV-coord
rlm@156 110 "Get the uv-cooridnates of the triangle's verticies."
rlm@156 111 [#^Mesh mesh width height triangle-index]
rlm@156 112 (map (fn [[u v]] (vector (* width u) (* height v)))
rlm@156 113 (map (partial vertex-UV-coord mesh)
rlm@156 114 (triangle-vertex-indices mesh triangle-index))))
rlm@156 115
rlm@156 116 (defn same-side?
rlm@156 117 "Given the points p1 and p2 and the reference point ref, is point p
rlm@156 118 on the same side of the line that goes through p1 and p2 as ref is?"
rlm@156 119 [p1 p2 ref p]
rlm@156 120 (<=
rlm@156 121 0
rlm@156 122 (.dot
rlm@156 123 (.cross (.subtract p2 p1) (.subtract p p1))
rlm@156 124 (.cross (.subtract p2 p1) (.subtract ref p1)))))
rlm@156 125
rlm@156 126 (defn triangle-seq [#^Triangle tri]
rlm@156 127 [(.get1 tri) (.get2 tri) (.get3 tri)])
rlm@156 128
rlm@156 129 (defn vector3f-seq [#^Vector3f v]
rlm@156 130 [(.getX v) (.getY v) (.getZ v)])
rlm@156 131
rlm@156 132 (defn inside-triangle?
rlm@156 133 "Is the point inside the triangle?"
rlm@156 134 {:author "Dylan Holmes"}
rlm@156 135 [#^Triangle tri #^Vector3f p]
rlm@156 136 (let [[vert-1 vert-2 vert-3] (triangle-seq tri)]
rlm@156 137 (and
rlm@156 138 (same-side? vert-1 vert-2 vert-3 p)
rlm@156 139 (same-side? vert-2 vert-3 vert-1 p)
rlm@156 140 (same-side? vert-3 vert-1 vert-2 p))))
rlm@156 141
rlm@156 142 (defn triangle->matrix4f
rlm@156 143 "Converts the triangle into a 4x4 matrix: The first three columns
rlm@156 144 contain the vertices of the triangle; the last contains the unit
rlm@156 145 normal of the triangle. The bottom row is filled with 1s."
rlm@156 146 [#^Triangle t]
rlm@156 147 (let [mat (Matrix4f.)
rlm@156 148 [vert-1 vert-2 vert-3]
rlm@156 149 ((comp vec map) #(.get t %) (range 3))
rlm@156 150 unit-normal (do (.calculateNormal t)(.getNormal t))
rlm@156 151 vertices [vert-1 vert-2 vert-3 unit-normal]]
rlm@156 152 (dorun
rlm@156 153 (for [row (range 4) col (range 3)]
rlm@37 154 (do
rlm@156 155 (.set mat col row (.get (vertices row)col))
rlm@156 156 (.set mat 3 row 1))))
rlm@156 157 mat))
rlm@156 158
rlm@156 159 (defn triangle-transformation
rlm@156 160 "Returns the affine transformation that converts each vertex in the
rlm@156 161 first triangle into the corresponding vertex in the second
rlm@156 162 triangle."
rlm@156 163 [#^Triangle tri-1 #^Triangle tri-2]
rlm@156 164 (.mult
rlm@156 165 (triangle->matrix4f tri-2)
rlm@156 166 (.invert (triangle->matrix4f tri-1))))
rlm@156 167
rlm@156 168 (defn point->vector2f [[u v]]
rlm@156 169 (Vector2f. u v))
rlm@156 170
rlm@156 171 (defn vector2f->vector3f [v]
rlm@156 172 (Vector3f. (.getX v) (.getY v) 0))
rlm@156 173
rlm@156 174 (defn map-triangle [f #^Triangle tri]
rlm@156 175 (Triangle.
rlm@156 176 (f 0 (.get1 tri))
rlm@156 177 (f 1 (.get2 tri))
rlm@156 178 (f 2 (.get3 tri))))
rlm@156 179
rlm@156 180 (defn points->triangle
rlm@156 181 "Convert a list of points into a triangle."
rlm@156 182 [points]
rlm@156 183 (apply #(Triangle. %1 %2 %3)
rlm@156 184 (map (fn [point]
rlm@156 185 (let [point (vec point)]
rlm@156 186 (Vector3f. (get point 0 0)
rlm@156 187 (get point 1 0)
rlm@156 188 (get point 2 0))))
rlm@156 189 (take 3 points))))
rlm@156 190
rlm@156 191 (defn convex-bounds
rlm@178 192 "Returns the smallest square containing the given vertices, as a
rlm@178 193 vector of integers [left top width height]."
rlm@156 194 [uv-verts]
rlm@156 195 (let [xs (map first uv-verts)
rlm@156 196 ys (map second uv-verts)
rlm@156 197 x0 (Math/floor (apply min xs))
rlm@156 198 y0 (Math/floor (apply min ys))
rlm@156 199 x1 (Math/ceil (apply max xs))
rlm@156 200 y1 (Math/ceil (apply max ys))]
rlm@156 201 [x0 y0 (- x1 x0) (- y1 y0)]))
rlm@156 202
rlm@156 203 (defn sensors-in-triangle
rlm@178 204 "Locate the touch sensors in the triangle, returning a map of their
rlm@178 205 UV and geometry-relative coordinates."
rlm@156 206 [image mesh tri-index]
rlm@156 207 (let [width (.getWidth image)
rlm@156 208 height (.getHeight image)
rlm@156 209 UV-vertex-coords (triangle-UV-coord mesh width height tri-index)
rlm@156 210 bounds (convex-bounds UV-vertex-coords)
rlm@156 211
rlm@156 212 cutout-triangle (points->triangle UV-vertex-coords)
rlm@156 213 UV-sensor-coords
rlm@156 214 (filter (comp (partial inside-triangle? cutout-triangle)
rlm@156 215 (fn [[u v]] (Vector3f. u v 0)))
rlm@156 216 (white-coordinates image bounds))
rlm@156 217 UV->geometry (triangle-transformation
rlm@156 218 cutout-triangle
rlm@156 219 (triangle mesh tri-index))
rlm@156 220 geometry-sensor-coords
rlm@156 221 (map (fn [[u v]] (.mult UV->geometry (Vector3f. u v 0)))
rlm@156 222 UV-sensor-coords)]
rlm@156 223 {:UV UV-sensor-coords :geometry geometry-sensor-coords}))
rlm@156 224
rlm@156 225 (defn-memo locate-feelers
rlm@178 226 "Search the geometry's tactile UV profile for touch sensors,
rlm@178 227 returning their positions in geometry-relative coordinates."
rlm@156 228 [#^Geometry geo]
rlm@156 229 (let [mesh (.getMesh geo)
rlm@156 230 num-triangles (.getTriangleCount mesh)]
rlm@178 231 (if-let [image (tactile-sensor-profile geo)]
rlm@156 232 (map
rlm@156 233 (partial sensors-in-triangle image mesh)
rlm@156 234 (range num-triangles))
rlm@156 235 (repeat (.getTriangleCount mesh) {:UV nil :geometry nil}))))
rlm@156 236
rlm@178 237 (defn-memo touch-topology
rlm@178 238 "Return a sequence of vectors of the form [x y] describing the
rlm@178 239 \"topology\" of the tactile sensors. Points that are close together
rlm@178 240 in the touch-topology are generally close together in the simulation."
rlm@178 241 [#^Gemoetry geo]
rlm@156 242 (vec (collapse (reduce concat (map :UV (locate-feelers geo))))))
rlm@156 243
rlm@178 244 (defn-memo feeler-coordinates
rlm@178 245 "The location of the touch sensors in world-space coordinates."
rlm@178 246 [#^Geometry geo]
rlm@156 247 (vec (map :geometry (locate-feelers geo))))
rlm@156 248
rlm@178 249 (defn touch-fn
rlm@178 250 "Returns a function which returns tactile sensory data when called
rlm@178 251 inside a running simulation."
rlm@178 252 [#^Geometry geo]
rlm@156 253 (let [feeler-coords (feeler-coordinates geo)
rlm@156 254 tris (triangles geo)
rlm@156 255 limit 0.1
rlm@156 256 ;;results (CollisionResults.)
rlm@156 257 ]
rlm@156 258 (if (empty? (touch-topology geo))
rlm@156 259 nil
rlm@156 260 (fn [node]
rlm@156 261 (let [sensor-origins
rlm@156 262 (map
rlm@156 263 #(map (partial local-to-world geo) %)
rlm@156 264 feeler-coords)
rlm@156 265 triangle-normals
rlm@156 266 (map (partial get-ray-direction geo)
rlm@156 267 tris)
rlm@156 268 rays
rlm@156 269 (flatten
rlm@156 270 (map (fn [origins norm]
rlm@156 271 (map #(doto (Ray. % norm)
rlm@156 272 (.setLimit limit)) origins))
rlm@156 273 sensor-origins triangle-normals))]
rlm@156 274 (vector
rlm@156 275 (touch-topology geo)
rlm@156 276 (vec
rlm@156 277 (for [ray rays]
rlm@156 278 (do
rlm@156 279 (let [results (CollisionResults.)]
rlm@156 280 (.collideWith node ray results)
rlm@156 281 (let [touch-objects
rlm@156 282 (filter #(not (= geo (.getGeometry %)))
rlm@156 283 results)]
rlm@156 284 (- 255
rlm@156 285 (if (empty? touch-objects) 255
rlm@156 286 (rem
rlm@156 287 (int
rlm@156 288 (* 255 (/ (.getDistance
rlm@156 289 (first touch-objects)) limit)))
rlm@156 290 256))))))))))))))
rlm@156 291
rlm@178 292 (defn touch!
rlm@178 293 "Endow the creature with the sense of touch. Returns a sequence of
rlm@178 294 functions, one for each body part with a tactile-sensor-proile,
rlm@178 295 each of which when called returns sensory data for that body part."
rlm@178 296 [#^Node creature]
rlm@178 297 (filter
rlm@178 298 (comp not nil?)
rlm@178 299 (map touch-fn
rlm@178 300 (filter #(isa? (class %) Geometry)
rlm@178 301 (node-seq creature)))))
rlm@156 302
rlm@156 303
rlm@37 304 #+end_src
rlm@37 305
rlm@37 306
rlm@37 307 * Example
rlm@37 308
rlm@66 309 #+name: touch-test
rlm@37 310 #+begin_src clojure
rlm@68 311 (ns cortex.test.touch
rlm@59 312 (:use (cortex world util touch))
rlm@59 313 (:import
rlm@59 314 com.jme3.scene.shape.Sphere
rlm@59 315 com.jme3.math.ColorRGBA
rlm@59 316 com.jme3.math.Vector3f
rlm@59 317 com.jme3.material.RenderState$BlendMode
rlm@59 318 com.jme3.renderer.queue.RenderQueue$Bucket
rlm@59 319 com.jme3.scene.shape.Box
rlm@68 320 com.jme3.scene.Node))
rlm@39 321
rlm@7 322 (defn ray-origin-debug
rlm@9 323 [ray color]
rlm@7 324 (make-shape
rlm@20 325 (assoc base-shape
rlm@20 326 :shape (Sphere. 5 5 0.05)
rlm@20 327 :name "arrow"
rlm@20 328 :color color
rlm@20 329 :texture false
rlm@20 330 :physical? false
rlm@20 331 :position
rlm@20 332 (.getOrigin ray))))
rlm@6 333
rlm@9 334 (defn ray-debug [ray color]
rlm@6 335 (make-shape
rlm@6 336 (assoc
rlm@6 337 base-shape
rlm@6 338 :name "debug-ray"
rlm@6 339 :physical? false
rlm@6 340 :shape (com.jme3.scene.shape.Line.
rlm@6 341 (.getOrigin ray)
rlm@6 342 (.add
rlm@6 343 (.getOrigin ray)
rlm@6 344 (.mult (.getDirection ray)
rlm@6 345 (float (.getLimit ray))))))))
rlm@6 346
rlm@6 347
rlm@10 348 (defn contact-color [contacts]
rlm@10 349 (case contacts
rlm@10 350 0 ColorRGBA/Gray
rlm@37 351 1 ColorRGBA/Red
rlm@10 352 2 ColorRGBA/Green
rlm@10 353 3 ColorRGBA/Yellow
rlm@10 354 4 ColorRGBA/Orange
rlm@10 355 5 ColorRGBA/Red
rlm@10 356 6 ColorRGBA/Magenta
rlm@10 357 7 ColorRGBA/Pink
rlm@10 358 8 ColorRGBA/White))
rlm@6 359
rlm@14 360 (defn update-ray-debug [node ray contacts]
rlm@14 361 (let [origin (.getChild node 0)]
rlm@14 362 (.setLocalTranslation origin (.getOrigin ray))
rlm@14 363 (.setColor (.getMaterial origin) "Color" (contact-color contacts))))
rlm@14 364
rlm@13 365 (defn init-node
rlm@13 366 [debug-node rays]
rlm@12 367 (.detachAllChildren debug-node)
rlm@13 368 (dorun
rlm@13 369 (for [ray rays]
rlm@13 370 (do
rlm@13 371 (.attachChild
rlm@13 372 debug-node
rlm@13 373 (doto (Node.)
rlm@14 374 (.attachChild (ray-origin-debug ray ColorRGBA/Gray))
rlm@20 375 (.attachChild (ray-debug ray ColorRGBA/Gray))
rlm@14 376 ))))))
rlm@14 377
rlm@13 378 (defn manage-ray-debug-node [debug-node geom touch-data limit]
rlm@13 379 (let [rays (normal-rays limit geom)]
rlm@13 380 (if (not= (count (.getChildren debug-node)) (count touch-data))
rlm@13 381 (init-node debug-node rays))
rlm@13 382 (dorun
rlm@13 383 (for [n (range (count touch-data))]
rlm@14 384 (update-ray-debug
rlm@14 385 (.getChild debug-node n) (nth rays n) (nth touch-data n))))))
rlm@12 386
rlm@0 387 (defn transparent-sphere []
rlm@0 388 (doto
rlm@0 389 (make-shape
rlm@0 390 (merge base-shape
rlm@0 391 {:position (Vector3f. 0 2 0)
rlm@0 392 :name "the blob."
rlm@0 393 :material "Common/MatDefs/Misc/Unshaded.j3md"
rlm@0 394 :texture "Textures/purpleWisp.png"
rlm@0 395 :physical? true
rlm@0 396 :mass 70
rlm@0 397 :color ColorRGBA/Blue
rlm@0 398 :shape (Sphere. 10 10 1)}))
rlm@0 399 (-> (.getMaterial)
rlm@0 400 (.getAdditionalRenderState)
rlm@0 401 (.setBlendMode RenderState$BlendMode/Alpha))
rlm@0 402 (.setQueueBucket RenderQueue$Bucket/Transparent)))
rlm@0 403
rlm@0 404 (defn transparent-box []
rlm@0 405 (doto
rlm@0 406 (make-shape
rlm@0 407 (merge base-shape
rlm@0 408 {:position (Vector3f. 0 2 0)
rlm@10 409 :name "box"
rlm@0 410 :material "Common/MatDefs/Misc/Unshaded.j3md"
rlm@0 411 :texture "Textures/purpleWisp.png"
rlm@0 412 :physical? true
rlm@0 413 :mass 70
rlm@0 414 :color ColorRGBA/Blue
rlm@0 415 :shape (Box. 1 1 1)}))
rlm@0 416 (-> (.getMaterial)
rlm@0 417 (.getAdditionalRenderState)
rlm@0 418 (.setBlendMode RenderState$BlendMode/Alpha))
rlm@0 419 (.setQueueBucket RenderQueue$Bucket/Transparent)))
rlm@0 420
rlm@6 421 (defn transparent-floor []
rlm@6 422 (doto
rlm@6 423 (box 5 0.2 5 :mass 0 :position (Vector3f. 0 -2 0)
rlm@6 424 :material "Common/MatDefs/Misc/Unshaded.j3md"
rlm@10 425 :texture "Textures/redWisp.png"
rlm@10 426 :name "floor")
rlm@6 427 (-> (.getMaterial)
rlm@6 428 (.getAdditionalRenderState)
rlm@6 429 (.setBlendMode RenderState$BlendMode/Alpha))
rlm@6 430 (.setQueueBucket RenderQueue$Bucket/Transparent)))
rlm@6 431
rlm@69 432 (defn test-skin
rlm@69 433 "Testing touch:
rlm@69 434 you should see a ball which responds to the table
rlm@69 435 and whatever balls hit it."
rlm@69 436 []
rlm@0 437 (let [b
rlm@58 438 ;;(transparent-box)
rlm@58 439 (transparent-sphere)
rlm@10 440 ;;(sphere)
rlm@58 441 f (transparent-floor)
rlm@6 442 debug-node (Node.)
rlm@12 443 node (doto (Node.) (.attachChild b) (.attachChild f))
rlm@12 444 root-node (doto (Node.) (.attachChild node)
rlm@12 445 (.attachChild debug-node))
rlm@12 446 ]
rlm@0 447
rlm@0 448 (world
rlm@12 449 root-node
rlm@15 450 {"key-return" (fire-cannon-ball node)}
rlm@0 451 (fn [world]
rlm@20 452 ;; (Capture/SimpleCaptureVideo
rlm@20 453 ;; world
rlm@20 454 ;; (file-str "/home/r/proj/cortex/tmp/blob.avi"))
rlm@20 455 ;; (no-logging)
rlm@20 456 ;;(enable-debug world)
rlm@20 457 ;; (set-accuracy world (/ 1 60))
rlm@58 458 )
rlm@58 459
rlm@0 460 (fn [& _]
rlm@19 461 (let [sensitivity 0.2
rlm@18 462 touch-data (touch-percieve sensitivity b node)]
rlm@68 463 (manage-ray-debug-node debug-node b touch-data sensitivity))
rlm@0 464 ))))
rlm@0 465
rlm@37 466
rlm@0 467 #+end_src
rlm@0 468
rlm@0 469
rlm@10 470
rlm@10 471
rlm@6 472
rlm@0 473 * COMMENT code generation
rlm@39 474 #+begin_src clojure :tangle ../src/cortex/touch.clj
rlm@0 475 <<skin-main>>
rlm@0 476 #+end_src
rlm@0 477
rlm@68 478 #+begin_src clojure :tangle ../src/cortex/test/touch.clj
rlm@39 479 <<touch-test>>
rlm@39 480 #+end_src
rlm@39 481
rlm@0 482
rlm@0 483
rlm@0 484
rlm@32 485
rlm@32 486