Mercurial > cortex

annotate org/touch.org @ 178:6fba17a74a57

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