view org/touch.org @ 240:6961377c4554

saving progress...
author Robert McIntyre <rlm@mit.edu>
date Sun, 12 Feb 2012 13:25:42 -0700
parents 78a640e3bc55
children f2e583be8584
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=. I call
54 these "hairs" /feelers/.
56 #+name: meta-data
57 #+begin_src clojure
58 (defn tactile-sensor-profile
59 "Return the touch-sensor distribution image in BufferedImage format,
60 or nil if it does not exist."
61 [#^Geometry obj]
62 (if-let [image-path (meta-data obj "touch")]
63 (load-image image-path)))
65 (defn tactile-scale
66 "Return the maximum length of a hair. All hairs are scalled between
67 0.0 and this length, depending on their color. Black is 0, and
68 white is maximum length, and everything in between is scalled
69 linearlly. Default scale is 0.01 jMonkeyEngine units."
70 [#^Geometry obj]
71 (if-let [scale (meta-data obj "scale")]
72 scale 0.1))
73 #+end_src
75 ** TODO add image showing example touch-uv map
76 ** TODO add metadata display for worm
79 * Skin Creation
80 * TODO get the actual lengths for each hair
82 #+begin_src clojure
83 pixel-triangles
84 xyz-triangles
85 conversions (map triangles->affine-transform pixel-triangles
86 xyz-triangles)
88 #+end_src
91 =(touch-kernel)= generates the functions which implement the sense of
92 touch for a creature. These functions must do 6 things to obtain touch
93 data.
95 - Get the tactile profile image and scale paramaters which describe
96 the layout of feelers along the object's surface.
97 =(tactile-sensor-profile)=, =(tactile-scale)=
99 - Get the lengths of each feeler by analyzing the color of the
100 pixels in the tactile profile image.
101 NOT IMPLEMENTED YET
103 - Find the triangles which make up the mesh in pixel-space and in
104 world-space.
105 =(triangles)= =(pixel-triangles)=
107 - Find the coordinates of each pixel in pixel space. These
108 coordinates are used to make the touch-topology.
109 =(feeler-pixel-coords)=
111 - Find the coordinates of each pixel in world-space. These
112 coordinates are the origins of the feelers. =(feeler-origins)=
114 - Calculate the normals of the triangles in world space, and add
115 them to each of the origins of the feelers. These are the
116 normalized coordinates of the tips of the feelers.
117 For both of these, =(feeler-tips)=
119 - Generate some sort of topology for the sensors.
120 =(touch-topology)=
122 #+begin_src clojure
127 #+end_src
131 #+name: kernel
132 #+begin_src clojure
133 (in-ns 'cortex.touch)
135 (declare touch-topology feelers set-ray)
137 (defn touch-kernel
138 "Constructs a function which will return tactile sensory data from
139 'geo when called from inside a running simulation"
140 [#^Geometry geo]
141 (let [[ray-reference-origins
142 ray-reference-tips
143 ray-lengths] (feelers geo)
144 current-rays (map (fn [] (Ray.)) ray-reference-origins)
145 topology (touch-topology geo)]
146 (if (empty? ray-reference-origins) nil
147 (fn [node]
148 (let [transform (.getWorldMatrix geo)]
149 (dorun
150 (map (fn [ray ref-origin ref-tip length]
151 (set-ray ray transform ref-origin ref-tip length))
152 current-rays ray-reference-origins
153 ray-reference-tips ray-lengths))
154 (vector
155 topology
156 (vec
157 (for [ray current-rays]
158 (do
159 (let [results (CollisionResults.)]
160 (.collideWith node ray results)
161 (let [touch-objects
162 (filter #(not (= geo (.getGeometry %)))
163 results)]
164 [(if (empty? touch-objects)
165 (.getLimit ray)
166 (.getDistance (first touch-objects)))
167 (.getLimit ray)])))))))))))
169 (defn touch-kernel*
170 "Returns a function which returns tactile sensory data when called
171 inside a running simulation."
172 [#^Geometry geo]
173 (let [feeler-coords (feeler-coordinates geo)
174 tris (triangles geo)
175 limit (tactile-scale geo)]
176 (if (empty? (touch-topology geo))
177 nil
178 (fn [node]
179 (let [sensor-origins
180 (map
181 #(map (partial local-to-world geo) %)
182 feeler-coords)
183 triangle-normals
184 (map (partial get-ray-direction geo)
185 tris)
186 rays
187 (flatten
188 (map (fn [origins norm]
189 (map #(doto (Ray. % norm)
190 (.setLimit limit)) origins))
191 sensor-origins triangle-normals))]
192 (vector
193 (touch-topology geo)
194 (vec
195 (for [ray rays]
196 (do
197 (let [results (CollisionResults.)]
198 (.collideWith node ray results)
199 (let [touch-objects
200 (filter #(not (= geo (.getGeometry %)))
201 results)]
202 [(if (empty? touch-objects)
203 limit (.getDistance (first touch-objects)))
204 limit])))))))))))
206 (defn touch!
207 "Endow the creature with the sense of touch. Returns a sequence of
208 functions, one for each body part with a tactile-sensor-proile,
209 each of which when called returns sensory data for that body part."
210 [#^Node creature]
211 (filter
212 (comp not nil?)
213 (map touch-kernel
214 (filter #(isa? (class %) Geometry)
215 (node-seq creature)))))
216 #+end_src
218 * Sensor Related Functions
220 These functions analyze the touch-sensor-profile image convert the
221 location of each touch sensor from pixel coordinates to UV-coordinates
222 and XYZ-coordinates.
224 #+name: sensors
225 #+begin_src clojure
226 (in-ns 'cortex.touch)
228 (defn feeler-pixel-coords
229 "Returns the coordinates of the feelers in pixel space in lists, one
230 list for each triangle, ordered in the same way as (triangles) and
231 (pixel-triangles)."
232 [#^Geometry geo image]
233 (map
234 (fn [pixel-triangle]
235 (filter
236 (fn [coord]
237 (inside-triangle? (->triangle pixel-triangle)
238 (->vector3f coord)))
239 (white-coordinates image (convex-bounds pixel-triangle))))
240 (pixel-triangles geo image)))
242 (defn feeler-origins [#^Geometry geo image]
243 (let [transforms
244 (map #(triangles->affine-transform
245 (->triangle %1) (->triangle %2))
246 (pixel-triangles geo image)
247 (triangles geo))]
248 (mapcat (fn [transform coords]
249 (map #(.mult transform (->vector3f %)) coords))
250 transforms (feeler-pixel-coords geo image))))
252 (defn feeler-tips [#^Geometry geo image]
253 (let [origins (feeler-origins geo image)]
254 (
256 )
260 (defn sensors-in-triangle
261 "Locate the touch sensors in the triangle, returning a map of their
262 UV and geometry-relative coordinates."
263 [image mesh tri-index]
264 (let [width (.getWidth image)
265 height (.getHeight image)
266 UV-vertex-coords (triangle-UV-coord mesh width height tri-index)
267 bounds (convex-bounds UV-vertex-coords)
269 cutout-triangle (points->triangle UV-vertex-coords)
270 UV-sensor-coords
271 (filter (comp (partial inside-triangle? cutout-triangle)
272 (fn [[u v]] (Vector3f. u v 0)))
273 (white-coordinates image bounds))
274 UV->geometry (triangle-transformation
275 cutout-triangle
276 (mesh-triangle mesh tri-index))
277 geometry-sensor-coords
278 (map (fn [[u v]] (.mult UV->geometry (Vector3f. u v 0)))
279 UV-sensor-coords)]
280 {:UV UV-sensor-coords :geometry geometry-sensor-coords}))
282 (defn-memo locate-feelers
283 "Search the geometry's tactile UV profile for touch sensors,
284 returning their positions in geometry-relative coordinates."
285 [#^Geometry geo]
286 (let [mesh (.getMesh geo)
287 num-triangles (.getTriangleCount mesh)]
288 (if-let [image (tactile-sensor-profile geo)]
289 (map
290 (partial sensors-in-triangle image mesh)
291 (range num-triangles))
292 (repeat (.getTriangleCount mesh) {:UV nil :geometry nil}))))
294 (defn-memo touch-topology
295 "Return a sequence of vectors of the form [x y] describing the
296 \"topology\" of the tactile sensors. Points that are close together
297 in the touch-topology are generally close together in the simulation."
298 [#^Gemoetry geo]
299 (vec (collapse (reduce concat (map :UV (locate-feelers geo))))))
301 (defn-memo feeler-coordinates
302 "The location of the touch sensors in world-space coordinates."
303 [#^Geometry geo]
304 (vec (map :geometry (locate-feelers geo))))
305 #+end_src
310 * Visualizing Touch
311 #+name: visualization
312 #+begin_src clojure
313 (in-ns 'cortex.touch)
315 (defn touch->gray
316 "Convert a pair of [distance, max-distance] into a grayscale pixel"
317 [distance max-distance]
318 (gray
319 (- 255
320 (rem
321 (int
322 (* 255 (/ distance max-distance)))
323 256))))
325 (defn view-touch
326 "Creates a function which accepts a list of touch sensor-data and
327 displays each element to the screen."
328 []
329 (view-sense
330 (fn
331 [[coords sensor-data]]
332 (let [image (points->image coords)]
333 (dorun
334 (for [i (range (count coords))]
335 (.setRGB image ((coords i) 0) ((coords i) 1)
336 (apply touch->gray (sensor-data i)))))
337 image))))
338 #+end_src
342 * Triangle Manipulation Functions
344 The rigid bodies which make up a creature have an underlying
345 =Geometry=, which is a =Mesh= plus a =Material= and other important
346 data involved with displaying the body.
348 A =Mesh= is composed of =Triangles=, and each =Triangle= has three
349 verticies which have coordinates in XYZ space and UV space.
351 Here, =(triangles)= gets all the triangles which compose a mesh, and
352 =(triangle-UV-coord)= returns the the UV coordinates of the verticies
353 of a triangle.
355 #+name: triangles-1
356 #+begin_src clojure
357 (in-ns 'cortex.touch)
359 (defn vector3f-seq [#^Vector3f v]
360 [(.getX v) (.getY v) (.getZ v)])
362 (defn triangle-seq [#^Triangle tri]
363 [(vector3f-seq (.get1 tri))
364 (vector3f-seq (.get2 tri))
365 (vector3f-seq (.get3 tri))])
367 (defn ->vector3f
368 ([coords] (Vector3f. (nth coords 0 0)
369 (nth coords 1 0)
370 (nth coords 2 0))))
372 (defn ->triangle [points]
373 (apply #(Triangle. %1 %2 %3) (map ->vector3f points)))
375 (defn triangle
376 "Get the triangle specified by triangle-index from the mesh within
377 bounds."
378 [#^Geometry geo triangle-index]
379 (triangle-seq
380 (let [scratch (Triangle.)]
381 (.getTriangle (.getMesh geo) triangle-index scratch) scratch)))
383 (defn triangles
384 "Return a sequence of all the Triangles which compose a given
385 Geometry."
386 [#^Geometry geo]
387 (map (partial triangle geo) (range (.getTriangleCount (.getMesh geo)))))
389 (defn triangle-vertex-indices
390 "Get the triangle vertex indices of a given triangle from a given
391 mesh."
392 [#^Mesh mesh triangle-index]
393 (let [indices (int-array 3)]
394 (.getTriangle mesh triangle-index indices)
395 (vec indices)))
397 (defn vertex-UV-coord
398 "Get the UV-coordinates of the vertex named by vertex-index"
399 [#^Mesh mesh vertex-index]
400 (let [UV-buffer
401 (.getData
402 (.getBuffer
403 mesh
404 VertexBuffer$Type/TexCoord))]
405 [(.get UV-buffer (* vertex-index 2))
406 (.get UV-buffer (+ 1 (* vertex-index 2)))]))
408 (defn pixel-triangle [#^Geometry geo image index]
409 (let [mesh (.getMesh geo)
410 width (.getWidth image)
411 height (.getHeight image)]
412 (vec (map (fn [[u v]] (vector (* width u) (* height v)))
413 (map (partial vertex-UV-coord mesh)
414 (triangle-vertex-indices mesh index))))))
416 (defn pixel-triangles [#^Geometry geo image]
417 (let [height (.getHeight image)
418 width (.getWidth image)]
419 (map (partial pixel-triangle geo image)
420 (range (.getTriangleCount (.getMesh geo))))))
422 #+end_src
424 * Triangle Affine Transforms
426 The position of each hair is stored in a 2D image in UV
427 coordinates. To place the hair in 3D space we must convert from UV
428 coordinates to XYZ coordinates. Each =Triangle= has coordinates in
429 both UV-space and XYZ-space, which defines a unique [[http://mathworld.wolfram.com/AffineTransformation.html ][Affine Transform]]
430 for translating any coordinate within the UV triangle to the
431 cooresponding coordinate in the XYZ triangle.
433 #+name: triangles-3
434 #+begin_src clojure
435 (defn triangle->matrix4f
436 "Converts the triangle into a 4x4 matrix: The first three columns
437 contain the vertices of the triangle; the last contains the unit
438 normal of the triangle. The bottom row is filled with 1s."
439 [#^Triangle t]
440 (let [mat (Matrix4f.)
441 [vert-1 vert-2 vert-3]
442 ((comp vec map) #(.get t %) (range 3))
443 unit-normal (do (.calculateNormal t)(.getNormal t))
444 vertices [vert-1 vert-2 vert-3 unit-normal]]
445 (dorun
446 (for [row (range 4) col (range 3)]
447 (do
448 (.set mat col row (.get (vertices row)col))
449 (.set mat 3 row 1))))
450 mat))
452 (defn triangles->affine-transform
453 "Returns the affine transformation that converts each vertex in the
454 first triangle into the corresponding vertex in the second
455 triangle."
456 [#^Triangle tri-1 #^Triangle tri-2]
457 (.mult
458 (triangle->matrix4f tri-2)
459 (.invert (triangle->matrix4f tri-1))))
460 #+end_src
463 * Schrapnel Conversion Functions
465 It is convienent to treat a =Triangle= as a sequence of verticies, and
466 a =Vector2f= and =Vector3f= as a sequence of floats. These conversion
467 functions make this easy. If these classes implemented =Iterable= then
468 this code would not be necessary. Hopefully they will in the future.
470 #+name: triangles-2
471 #+begin_src clojure
472 (defn point->vector2f [[u v]]
473 (Vector2f. u v))
475 (defn vector2f->vector3f [v]
476 (Vector3f. (.getX v) (.getY v) 0))
478 (defn map-triangle [f #^Triangle tri]
479 (Triangle.
480 (f 0 (.get1 tri))
481 (f 1 (.get2 tri))
482 (f 2 (.get3 tri))))
484 (defn points->triangle
485 "Convert a list of points into a triangle."
486 [points]
487 (apply #(Triangle. %1 %2 %3)
488 (map (fn [point]
489 (let [point (vec point)]
490 (Vector3f. (get point 0 0)
491 (get point 1 0)
492 (get point 2 0))))
493 (take 3 points))))
494 #+end_src
497 * Triangle Boundaries
499 For efficiency's sake I will divide the UV-image into small squares
500 which inscribe each UV-triangle, then extract the points which lie
501 inside the triangle and map them to 3D-space using
502 =(triangle-transform)= above. To do this I need a function,
503 =(inside-triangle?)=, which determines whether a point is inside a
504 triangle in 2D UV-space.
506 #+name: triangles-4
507 #+begin_src clojure
508 (defn convex-bounds
509 "Returns the smallest square containing the given vertices, as a
510 vector of integers [left top width height]."
511 [verts]
512 (let [xs (map first verts)
513 ys (map second verts)
514 x0 (Math/floor (apply min xs))
515 y0 (Math/floor (apply min ys))
516 x1 (Math/ceil (apply max xs))
517 y1 (Math/ceil (apply max ys))]
518 [x0 y0 (- x1 x0) (- y1 y0)]))
520 (defn same-side?
521 "Given the points p1 and p2 and the reference point ref, is point p
522 on the same side of the line that goes through p1 and p2 as ref is?"
523 [p1 p2 ref p]
524 (<=
525 0
526 (.dot
527 (.cross (.subtract p2 p1) (.subtract p p1))
528 (.cross (.subtract p2 p1) (.subtract ref p1)))))
530 (defn inside-triangle?
531 "Is the point inside the triangle?"
532 {:author "Dylan Holmes"}
533 [#^Triangle tri #^Vector3f p]
534 (let [[vert-1 vert-2 vert-3] [(.get1 tri) (.get2 tri) (.get3 tri)]]
535 (and
536 (same-side? vert-1 vert-2 vert-3 p)
537 (same-side? vert-2 vert-3 vert-1 p)
538 (same-side? vert-3 vert-1 vert-2 p))))
539 #+end_src
541 #+results: triangles-4
542 : #'cortex.touch/inside-triangle?
545 * Physics Collision Objects
547 The "hairs" are actually =Rays= which extend from a point on a
548 =Triangle= in the =Mesh= normal to the =Triangle's= surface.
550 #+name: rays
551 #+begin_src clojure
552 (defn get-ray-origin
553 "Return the origin which a Ray would have to have to be in the exact
554 center of a particular Triangle in the Geometry in World
555 Coordinates."
556 [geom tri]
557 (let [new (Vector3f.)]
558 (.calculateCenter tri)
559 (.localToWorld geom (.getCenter tri) new) new))
561 (defn get-ray-direction
562 "Return the direction which a Ray would have to have to be to point
563 normal to the Triangle, in coordinates relative to the center of the
564 Triangle."
565 [geom tri]
566 (let [n+c (Vector3f.)]
567 (.calculateNormal tri)
568 (.calculateCenter tri)
569 (.localToWorld
570 geom
571 (.add (.getCenter tri) (.getNormal tri)) n+c)
572 (.subtract n+c (get-ray-origin geom tri))))
573 #+end_src
574 * Headers
576 #+name: touch-header
577 #+begin_src clojure
578 (ns cortex.touch
579 "Simulate the sense of touch in jMonkeyEngine3. Enables any Geometry
580 to be outfitted with touch sensors with density determined by a UV
581 image. In this way a Geometry can know what parts of itself are
582 touching nearby objects. Reads specially prepared blender files to
583 construct this sense automatically."
584 {:author "Robert McIntyre"}
585 (:use (cortex world util sense))
586 (:use clojure.contrib.def)
587 (:import (com.jme3.scene Geometry Node Mesh))
588 (:import com.jme3.collision.CollisionResults)
589 (:import com.jme3.scene.VertexBuffer$Type)
590 (:import (com.jme3.math Triangle Vector3f Vector2f Ray Matrix4f)))
591 #+end_src
593 * Adding Touch to the Worm
595 #+name: test-touch
596 #+begin_src clojure
597 (ns cortex.test.touch
598 (:use (cortex world util sense body touch))
599 (:use cortex.test.body))
601 (cortex.import/mega-import-jme3)
603 (defn test-touch []
604 (let [the-worm (doto (worm) (body!))
605 touch (touch! the-worm)
606 touch-display (view-touch)]
607 (world (nodify [the-worm (floor)])
608 standard-debug-controls
610 (fn [world]
611 (light-up-everything world))
613 (fn [world tpf]
614 (touch-display (map #(% (.getRootNode world)) touch))))))
615 #+end_src
616 * Source Listing
617 * Next
620 * COMMENT Code Generation
621 #+begin_src clojure :tangle ../src/cortex/touch.clj
622 <<touch-header>>
623 <<meta-data>>
624 <<triangles-1>>
625 <<triangles-2>>
626 <<triangles-3>>
627 <<triangles-4>>
628 <<sensors>>
629 <<rays>>
630 <<kernel>>
631 <<visualization>>
632 #+end_src
635 #+begin_src clojure :tangle ../src/cortex/test/touch.clj
636 <<test-touch>>
637 #+end_src