view org/touch.org @ 235:be78d7bd6920

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