view org/touch.org @ 210:384ed3437948

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