annotate org/touch.org @ 232:b7762699eeb5

completed basic touch test
author Robert McIntyre <rlm@mit.edu>
date Sat, 11 Feb 2012 19:48:32 -0700
parents e29dd0024a9e
children f27c9fd9134d
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@229 9
rlm@229 10
rlm@37 11 * Touch
rlm@0 12
rlm@226 13 Touch is critical to navigation and spatial reasoning and as such I
rlm@226 14 need a simulated version of it to give to my AI creatures.
rlm@0 15
rlm@228 16 However, touch in my virtual can not exactly correspond to human touch
rlm@228 17 because my creatures are made out of completely rigid segments that
rlm@228 18 don't deform like human skin.
rlm@228 19
rlm@228 20 Human skin has a wide array of touch sensors, each of which speciliaze
rlm@228 21 in detecting different vibrational modes and pressures. These sensors
rlm@228 22 can integrate a vast expanse of skin (i.e. your entire palm), or a
rlm@228 23 tiny patch of skin at the tip of your finger. The hairs of the skin
rlm@228 24 help detect objects before they even come into contact with the skin
rlm@228 25 proper.
rlm@228 26
rlm@228 27 Instead of measuring deformation or vibration, I surround each rigid
rlm@228 28 part with a plenitude of hair-like objects which do not interact with
rlm@228 29 the physical world. Physical objects can pass through them with no
rlm@228 30 effect. The hairs are able to measure contact with other objects, and
rlm@228 31 constantly report how much of their extent is covered. So, even though
rlm@228 32 the creature's body parts do not deform, the hairs create a margin
rlm@228 33 around those body parts which achieves a sense of touch which is a
rlm@228 34 hybrid between a human's sense of deformation and sense from hairs.
rlm@228 35
rlm@228 36 Implementing touch in jMonkeyEngine follows a different techinal route
rlm@228 37 than vision and hearing. Those two senses piggybacked off
rlm@228 38 jMonkeyEngine's 3D audio and video rendering subsystems. To simulate
rlm@228 39 Touch, I use jMonkeyEngine's physics system to execute many small
rlm@229 40 collision detections, one for each "hair". The placement of the
rlm@229 41 "hairs" is determined by a UV-mapped image which shows where each hair
rlm@229 42 should be on the 3D surface of the body.
rlm@228 43
rlm@229 44
rlm@229 45 * Defining Touch Meta-Data in Blender
rlm@229 46
rlm@229 47 Each geometry can have a single UV map which describes the position
rlm@229 48 and length of the "hairs" which will constitute its sense of
rlm@229 49 touch. This image path is stored under the "touch" key. The image
rlm@229 50 itself is grayscale, with black meaning a hair length of 0 (no hair is
rlm@229 51 present) and white meaning a hair length of =scale=, which is a float
rlm@229 52 stored under the key "scale". If the pixel is gray then the resultant
rlm@229 53 hair length is linearly interpolated between 0 and =scale=.
rlm@229 54
rlm@231 55 #+name: meta-data
rlm@0 56 #+begin_src clojure
rlm@229 57 (defn tactile-sensor-profile
rlm@229 58 "Return the touch-sensor distribution image in BufferedImage format,
rlm@229 59 or nil if it does not exist."
rlm@229 60 [#^Geometry obj]
rlm@229 61 (if-let [image-path (meta-data obj "touch")]
rlm@229 62 (load-image image-path)))
rlm@228 63 #+end_src
rlm@156 64
rlm@229 65
rlm@229 66 ** TODO add image showing example touch-uv map
rlm@229 67 ** TODO add metadata display for worm
rlm@229 68
rlm@228 69 * Triangle Manipulation Functions
rlm@228 70
rlm@229 71 The rigid bodies which make up a creature have an underlying
rlm@229 72 =Geometry=, which is a =Mesh= plus a =Material= and other important
rlm@229 73 data involved with displaying the body.
rlm@229 74
rlm@229 75 A =Mesh= is composed of =Triangles=, and each =Triangle= has three
rlm@229 76 verticies which have coordinates in XYZ space and UV space.
rlm@229 77
rlm@229 78 Here, =(triangles)= gets all the triangles which compose a mesh, and
rlm@229 79 =(triangle-UV-coord)= returns the the UV coordinates of the verticies
rlm@229 80 of a triangle.
rlm@229 81
rlm@231 82 #+name: triangles-1
rlm@228 83 #+begin_src clojure
rlm@228 84 (defn triangles
rlm@228 85 "Return a sequence of all the Triangles which compose a given
rlm@228 86 Geometry."
rlm@228 87 [#^Geometry geom]
rlm@228 88 (let
rlm@228 89 [mesh (.getMesh geom)
rlm@228 90 triangles (transient [])]
rlm@228 91 (dorun
rlm@228 92 (for [n (range (.getTriangleCount mesh))]
rlm@228 93 (let [tri (Triangle.)]
rlm@228 94 (.getTriangle mesh n tri)
rlm@228 95 ;; (.calculateNormal tri)
rlm@228 96 ;; (.calculateCenter tri)
rlm@228 97 (conj! triangles tri))))
rlm@228 98 (persistent! triangles)))
rlm@228 99
rlm@228 100 (defn mesh-triangle
rlm@228 101 "Get the triangle specified by triangle-index from the mesh within
rlm@228 102 bounds."
rlm@228 103 [#^Mesh mesh triangle-index]
rlm@228 104 (let [scratch (Triangle.)]
rlm@228 105 (.getTriangle mesh triangle-index scratch)
rlm@228 106 scratch))
rlm@228 107
rlm@228 108 (defn triangle-vertex-indices
rlm@228 109 "Get the triangle vertex indices of a given triangle from a given
rlm@228 110 mesh."
rlm@228 111 [#^Mesh mesh triangle-index]
rlm@228 112 (let [indices (int-array 3)]
rlm@228 113 (.getTriangle mesh triangle-index indices)
rlm@228 114 (vec indices)))
rlm@228 115
rlm@228 116 (defn vertex-UV-coord
rlm@228 117 "Get the UV-coordinates of the vertex named by vertex-index"
rlm@228 118 [#^Mesh mesh vertex-index]
rlm@228 119 (let [UV-buffer
rlm@228 120 (.getData
rlm@228 121 (.getBuffer
rlm@228 122 mesh
rlm@228 123 VertexBuffer$Type/TexCoord))]
rlm@228 124 [(.get UV-buffer (* vertex-index 2))
rlm@228 125 (.get UV-buffer (+ 1 (* vertex-index 2)))]))
rlm@228 126
rlm@228 127 (defn triangle-UV-coord
rlm@228 128 "Get the UV-cooridnates of the triangle's verticies."
rlm@228 129 [#^Mesh mesh width height triangle-index]
rlm@228 130 (map (fn [[u v]] (vector (* width u) (* height v)))
rlm@228 131 (map (partial vertex-UV-coord mesh)
rlm@228 132 (triangle-vertex-indices mesh triangle-index))))
rlm@228 133 #+end_src
rlm@228 134
rlm@228 135 * Schrapnel Conversion Functions
rlm@229 136
rlm@229 137 It is convienent to treat a =Triangle= as a sequence of verticies, and
rlm@229 138 a =Vector2f= and =Vector3f= as a sequence of floats. These conversion
rlm@229 139 functions make this easy. If these classes implemented =Iterable= then
rlm@229 140 this code would not be necessary. Hopefully they will in the future.
rlm@229 141
rlm@231 142 #+name: triangles-2
rlm@228 143 #+begin_src clojure
rlm@228 144 (defn triangle-seq [#^Triangle tri]
rlm@228 145 [(.get1 tri) (.get2 tri) (.get3 tri)])
rlm@228 146
rlm@228 147 (defn vector3f-seq [#^Vector3f v]
rlm@228 148 [(.getX v) (.getY v) (.getZ v)])
rlm@228 149
rlm@228 150 (defn point->vector2f [[u v]]
rlm@228 151 (Vector2f. u v))
rlm@228 152
rlm@228 153 (defn vector2f->vector3f [v]
rlm@228 154 (Vector3f. (.getX v) (.getY v) 0))
rlm@228 155
rlm@228 156 (defn map-triangle [f #^Triangle tri]
rlm@228 157 (Triangle.
rlm@228 158 (f 0 (.get1 tri))
rlm@228 159 (f 1 (.get2 tri))
rlm@228 160 (f 2 (.get3 tri))))
rlm@228 161
rlm@228 162 (defn points->triangle
rlm@228 163 "Convert a list of points into a triangle."
rlm@228 164 [points]
rlm@228 165 (apply #(Triangle. %1 %2 %3)
rlm@228 166 (map (fn [point]
rlm@228 167 (let [point (vec point)]
rlm@228 168 (Vector3f. (get point 0 0)
rlm@228 169 (get point 1 0)
rlm@228 170 (get point 2 0))))
rlm@228 171 (take 3 points))))
rlm@228 172 #+end_src
rlm@228 173
rlm@228 174 * Triangle Affine Transforms
rlm@228 175
rlm@229 176 The position of each hair is stored in a 2D image in UV
rlm@229 177 coordinates. To place the hair in 3D space we must convert from UV
rlm@229 178 coordinates to XYZ coordinates. Each =Triangle= has coordinates in
rlm@229 179 both UV-space and XYZ-space, which defines a unique [[http://mathworld.wolfram.com/AffineTransformation.html ][Affine Transform]]
rlm@229 180 for translating any coordinate within the UV triangle to the
rlm@229 181 cooresponding coordinate in the XYZ triangle.
rlm@229 182
rlm@231 183 #+name: triangles-3
rlm@228 184 #+begin_src clojure
rlm@228 185 (defn triangle->matrix4f
rlm@228 186 "Converts the triangle into a 4x4 matrix: The first three columns
rlm@228 187 contain the vertices of the triangle; the last contains the unit
rlm@228 188 normal of the triangle. The bottom row is filled with 1s."
rlm@228 189 [#^Triangle t]
rlm@228 190 (let [mat (Matrix4f.)
rlm@228 191 [vert-1 vert-2 vert-3]
rlm@228 192 ((comp vec map) #(.get t %) (range 3))
rlm@228 193 unit-normal (do (.calculateNormal t)(.getNormal t))
rlm@228 194 vertices [vert-1 vert-2 vert-3 unit-normal]]
rlm@228 195 (dorun
rlm@228 196 (for [row (range 4) col (range 3)]
rlm@228 197 (do
rlm@228 198 (.set mat col row (.get (vertices row)col))
rlm@228 199 (.set mat 3 row 1))))
rlm@228 200 mat))
rlm@228 201
rlm@228 202 (defn triangle-transformation
rlm@228 203 "Returns the affine transformation that converts each vertex in the
rlm@228 204 first triangle into the corresponding vertex in the second
rlm@228 205 triangle."
rlm@228 206 [#^Triangle tri-1 #^Triangle tri-2]
rlm@228 207 (.mult
rlm@228 208 (triangle->matrix4f tri-2)
rlm@228 209 (.invert (triangle->matrix4f tri-1))))
rlm@228 210 #+end_src
rlm@228 211
rlm@229 212 * Triangle Boundaries
rlm@229 213
rlm@229 214 For efficiency's sake I will divide the UV-image into small squares
rlm@229 215 which inscribe each UV-triangle, then extract the points which lie
rlm@229 216 inside the triangle and map them to 3D-space using
rlm@229 217 =(triangle-transform)= above. To do this I need a function,
rlm@229 218 =(inside-triangle?)=, which determines whether a point is inside a
rlm@229 219 triangle in 2D UV-space.
rlm@228 220
rlm@231 221 #+name: triangles-4
rlm@228 222 #+begin_src clojure
rlm@229 223 (defn convex-bounds
rlm@229 224 "Returns the smallest square containing the given vertices, as a
rlm@229 225 vector of integers [left top width height]."
rlm@229 226 [uv-verts]
rlm@229 227 (let [xs (map first uv-verts)
rlm@229 228 ys (map second uv-verts)
rlm@229 229 x0 (Math/floor (apply min xs))
rlm@229 230 y0 (Math/floor (apply min ys))
rlm@229 231 x1 (Math/ceil (apply max xs))
rlm@229 232 y1 (Math/ceil (apply max ys))]
rlm@229 233 [x0 y0 (- x1 x0) (- y1 y0)]))
rlm@229 234
rlm@229 235 (defn same-side?
rlm@229 236 "Given the points p1 and p2 and the reference point ref, is point p
rlm@229 237 on the same side of the line that goes through p1 and p2 as ref is?"
rlm@229 238 [p1 p2 ref p]
rlm@229 239 (<=
rlm@229 240 0
rlm@229 241 (.dot
rlm@229 242 (.cross (.subtract p2 p1) (.subtract p p1))
rlm@229 243 (.cross (.subtract p2 p1) (.subtract ref p1)))))
rlm@229 244
rlm@229 245 (defn inside-triangle?
rlm@229 246 "Is the point inside the triangle?"
rlm@229 247 {:author "Dylan Holmes"}
rlm@229 248 [#^Triangle tri #^Vector3f p]
rlm@229 249 (let [[vert-1 vert-2 vert-3] (triangle-seq tri)]
rlm@229 250 (and
rlm@229 251 (same-side? vert-1 vert-2 vert-3 p)
rlm@229 252 (same-side? vert-2 vert-3 vert-1 p)
rlm@229 253 (same-side? vert-3 vert-1 vert-2 p))))
rlm@229 254 #+end_src
rlm@229 255
rlm@229 256
rlm@229 257
rlm@229 258 * Sensor Related Functions
rlm@229 259
rlm@229 260 These functions analyze the touch-sensor-profile image convert the
rlm@229 261 location of each touch sensor from pixel coordinates to UV-coordinates
rlm@229 262 and XYZ-coordinates.
rlm@229 263
rlm@231 264 #+name: sensors
rlm@229 265 #+begin_src clojure
rlm@229 266 (defn sensors-in-triangle
rlm@229 267 "Locate the touch sensors in the triangle, returning a map of their
rlm@229 268 UV and geometry-relative coordinates."
rlm@229 269 [image mesh tri-index]
rlm@229 270 (let [width (.getWidth image)
rlm@229 271 height (.getHeight image)
rlm@229 272 UV-vertex-coords (triangle-UV-coord mesh width height tri-index)
rlm@229 273 bounds (convex-bounds UV-vertex-coords)
rlm@229 274
rlm@229 275 cutout-triangle (points->triangle UV-vertex-coords)
rlm@229 276 UV-sensor-coords
rlm@229 277 (filter (comp (partial inside-triangle? cutout-triangle)
rlm@229 278 (fn [[u v]] (Vector3f. u v 0)))
rlm@229 279 (white-coordinates image bounds))
rlm@229 280 UV->geometry (triangle-transformation
rlm@229 281 cutout-triangle
rlm@229 282 (mesh-triangle mesh tri-index))
rlm@229 283 geometry-sensor-coords
rlm@229 284 (map (fn [[u v]] (.mult UV->geometry (Vector3f. u v 0)))
rlm@229 285 UV-sensor-coords)]
rlm@229 286 {:UV UV-sensor-coords :geometry geometry-sensor-coords}))
rlm@229 287
rlm@229 288 (defn-memo locate-feelers
rlm@229 289 "Search the geometry's tactile UV profile for touch sensors,
rlm@229 290 returning their positions in geometry-relative coordinates."
rlm@229 291 [#^Geometry geo]
rlm@229 292 (let [mesh (.getMesh geo)
rlm@229 293 num-triangles (.getTriangleCount mesh)]
rlm@229 294 (if-let [image (tactile-sensor-profile geo)]
rlm@229 295 (map
rlm@229 296 (partial sensors-in-triangle image mesh)
rlm@229 297 (range num-triangles))
rlm@229 298 (repeat (.getTriangleCount mesh) {:UV nil :geometry nil}))))
rlm@229 299
rlm@229 300 (defn-memo touch-topology
rlm@229 301 "Return a sequence of vectors of the form [x y] describing the
rlm@229 302 \"topology\" of the tactile sensors. Points that are close together
rlm@229 303 in the touch-topology are generally close together in the simulation."
rlm@229 304 [#^Gemoetry geo]
rlm@229 305 (vec (collapse (reduce concat (map :UV (locate-feelers geo))))))
rlm@229 306
rlm@229 307 (defn-memo feeler-coordinates
rlm@229 308 "The location of the touch sensors in world-space coordinates."
rlm@229 309 [#^Geometry geo]
rlm@229 310 (vec (map :geometry (locate-feelers geo))))
rlm@228 311 #+end_src
rlm@228 312
rlm@228 313 * Physics Collision Objects
rlm@230 314
rlm@230 315 The "hairs" are actually rays which extend from a point on a
rlm@230 316 =Triangle= in the =Mesh= normal to the =Triangle's= surface.
rlm@230 317
rlm@231 318 #+name: rays
rlm@228 319 #+begin_src clojure
rlm@228 320 (defn get-ray-origin
rlm@228 321 "Return the origin which a Ray would have to have to be in the exact
rlm@228 322 center of a particular Triangle in the Geometry in World
rlm@228 323 Coordinates."
rlm@228 324 [geom tri]
rlm@228 325 (let [new (Vector3f.)]
rlm@228 326 (.calculateCenter tri)
rlm@228 327 (.localToWorld geom (.getCenter tri) new) new))
rlm@228 328
rlm@228 329 (defn get-ray-direction
rlm@228 330 "Return the direction which a Ray would have to have to be to point
rlm@228 331 normal to the Triangle, in coordinates relative to the center of the
rlm@228 332 Triangle."
rlm@228 333 [geom tri]
rlm@228 334 (let [n+c (Vector3f.)]
rlm@228 335 (.calculateNormal tri)
rlm@228 336 (.calculateCenter tri)
rlm@228 337 (.localToWorld
rlm@228 338 geom
rlm@228 339 (.add (.getCenter tri) (.getNormal tri)) n+c)
rlm@228 340 (.subtract n+c (get-ray-origin geom tri))))
rlm@228 341 #+end_src
rlm@228 342
rlm@228 343
rlm@228 344 * Skin Creation
rlm@231 345 #+name: kernel
rlm@228 346 #+begin_src clojure
rlm@178 347 (defn touch-fn
rlm@178 348 "Returns a function which returns tactile sensory data when called
rlm@178 349 inside a running simulation."
rlm@178 350 [#^Geometry geo]
rlm@156 351 (let [feeler-coords (feeler-coordinates geo)
rlm@156 352 tris (triangles geo)
rlm@156 353 limit 0.1
rlm@156 354 ;;results (CollisionResults.)
rlm@156 355 ]
rlm@156 356 (if (empty? (touch-topology geo))
rlm@156 357 nil
rlm@156 358 (fn [node]
rlm@156 359 (let [sensor-origins
rlm@156 360 (map
rlm@156 361 #(map (partial local-to-world geo) %)
rlm@156 362 feeler-coords)
rlm@156 363 triangle-normals
rlm@156 364 (map (partial get-ray-direction geo)
rlm@156 365 tris)
rlm@156 366 rays
rlm@156 367 (flatten
rlm@156 368 (map (fn [origins norm]
rlm@156 369 (map #(doto (Ray. % norm)
rlm@156 370 (.setLimit limit)) origins))
rlm@156 371 sensor-origins triangle-normals))]
rlm@156 372 (vector
rlm@156 373 (touch-topology geo)
rlm@156 374 (vec
rlm@156 375 (for [ray rays]
rlm@156 376 (do
rlm@156 377 (let [results (CollisionResults.)]
rlm@156 378 (.collideWith node ray results)
rlm@156 379 (let [touch-objects
rlm@156 380 (filter #(not (= geo (.getGeometry %)))
rlm@156 381 results)]
rlm@156 382 (- 255
rlm@156 383 (if (empty? touch-objects) 255
rlm@156 384 (rem
rlm@156 385 (int
rlm@156 386 (* 255 (/ (.getDistance
rlm@156 387 (first touch-objects)) limit)))
rlm@156 388 256))))))))))))))
rlm@156 389
rlm@178 390 (defn touch!
rlm@178 391 "Endow the creature with the sense of touch. Returns a sequence of
rlm@178 392 functions, one for each body part with a tactile-sensor-proile,
rlm@178 393 each of which when called returns sensory data for that body part."
rlm@178 394 [#^Node creature]
rlm@178 395 (filter
rlm@178 396 (comp not nil?)
rlm@178 397 (map touch-fn
rlm@178 398 (filter #(isa? (class %) Geometry)
rlm@178 399 (node-seq creature)))))
rlm@228 400 #+end_src
rlm@156 401
rlm@228 402 * Visualizing Touch
rlm@231 403 #+name: visualization
rlm@228 404 #+begin_src clojure
rlm@188 405 (defn view-touch
rlm@189 406 "Creates a function which accepts a list of touch sensor-data and
rlm@189 407 displays each element to the screen."
rlm@188 408 []
rlm@187 409 (view-sense
rlm@187 410 (fn
rlm@187 411 [[coords sensor-data]]
rlm@187 412 (let [image (points->image coords)]
rlm@187 413 (dorun
rlm@187 414 (for [i (range (count coords))]
rlm@187 415 (.setRGB image ((coords i) 0) ((coords i) 1)
rlm@187 416 (gray (sensor-data i)))))
rlm@188 417 image))))
rlm@37 418 #+end_src
rlm@37 419
rlm@226 420 * Headers
rlm@231 421
rlm@231 422 #+name: touch-header
rlm@226 423 #+begin_src clojure
rlm@226 424 (ns cortex.touch
rlm@226 425 "Simulate the sense of touch in jMonkeyEngine3. Enables any Geometry
rlm@226 426 to be outfitted with touch sensors with density determined by a UV
rlm@226 427 image. In this way a Geometry can know what parts of itself are
rlm@226 428 touching nearby objects. Reads specially prepared blender files to
rlm@226 429 construct this sense automatically."
rlm@226 430 {:author "Robert McIntyre"}
rlm@226 431 (:use (cortex world util sense))
rlm@226 432 (:use clojure.contrib.def)
rlm@226 433 (:import (com.jme3.scene Geometry Node Mesh))
rlm@226 434 (:import com.jme3.collision.CollisionResults)
rlm@226 435 (:import com.jme3.scene.VertexBuffer$Type)
rlm@226 436 (:import (com.jme3.math Triangle Vector3f Vector2f Ray Matrix4f)))
rlm@226 437 #+end_src
rlm@37 438
rlm@232 439 * Adding Touch to the Worm
rlm@232 440
rlm@232 441 #+name: test-touch
rlm@232 442 #+begin_src clojure
rlm@232 443 (ns cortex.test.touch
rlm@232 444 (:use (cortex world util sense body touch))
rlm@232 445 (:use cortex.test.body))
rlm@232 446
rlm@232 447 (cortex.import/mega-import-jme3)
rlm@232 448
rlm@232 449 (defn test-touch []
rlm@232 450 (let [the-worm (doto (worm) (body!))
rlm@232 451 touch (touch! the-worm)
rlm@232 452 touch-display (view-touch)]
rlm@232 453 (world (nodify [the-worm (floor)])
rlm@232 454 standard-debug-controls
rlm@232 455
rlm@232 456 (fn [world]
rlm@232 457 (light-up-everything world))
rlm@232 458
rlm@232 459 (fn [world tpf]
rlm@232 460 (touch-display (map #(% (.getRootNode world)) touch))))))
rlm@232 461
rlm@232 462 #+end_src
rlm@228 463
rlm@228 464 * Source Listing
rlm@228 465 * Next
rlm@228 466
rlm@228 467
rlm@226 468 * COMMENT Code Generation
rlm@39 469 #+begin_src clojure :tangle ../src/cortex/touch.clj
rlm@231 470 <<touch-header>>
rlm@231 471 <<meta-data>>
rlm@231 472 <<triangles-1>>
rlm@231 473 <<triangles-2>>
rlm@231 474 <<triangles-3>>
rlm@231 475 <<triangles-4>>
rlm@231 476 <<sensors>>
rlm@231 477 <<rays>>
rlm@231 478 <<kernel>>
rlm@231 479 <<visualization>>
rlm@0 480 #+end_src
rlm@0 481
rlm@232 482
rlm@68 483 #+begin_src clojure :tangle ../src/cortex/test/touch.clj
rlm@232 484 <<test-touch>>
rlm@39 485 #+end_src
rlm@39 486
rlm@0 487
rlm@0 488
rlm@0 489
rlm@32 490
rlm@32 491
rlm@226 492