|
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@226
|
11 Touch is critical to navigation and spatial reasoning and as such I
|
|
rlm@226
|
12 need a simulated version of it to give to my AI creatures.
|
|
rlm@0
|
13
|
|
rlm@66
|
14 #+name: skin-main
|
|
rlm@0
|
15 #+begin_src clojure
|
|
rlm@226
|
16 (in-ns 'cortex.touch)
|
|
rlm@226
|
17
|
|
rlm@37
|
18 (defn triangles
|
|
rlm@37
|
19 "Return a sequence of all the Triangles which compose a given
|
|
rlm@37
|
20 Geometry."
|
|
rlm@37
|
21 [#^Geometry geom]
|
|
rlm@6
|
22 (let
|
|
rlm@6
|
23 [mesh (.getMesh geom)
|
|
rlm@6
|
24 triangles (transient [])]
|
|
rlm@6
|
25 (dorun
|
|
rlm@6
|
26 (for [n (range (.getTriangleCount mesh))]
|
|
rlm@6
|
27 (let [tri (Triangle.)]
|
|
rlm@6
|
28 (.getTriangle mesh n tri)
|
|
rlm@37
|
29 ;; (.calculateNormal tri)
|
|
rlm@37
|
30 ;; (.calculateCenter tri)
|
|
rlm@6
|
31 (conj! triangles tri))))
|
|
rlm@6
|
32 (persistent! triangles)))
|
|
rlm@6
|
33
|
|
rlm@7
|
34 (defn get-ray-origin
|
|
rlm@37
|
35 "Return the origin which a Ray would have to have to be in the exact
|
|
rlm@37
|
36 center of a particular Triangle in the Geometry in World
|
|
rlm@37
|
37 Coordinates."
|
|
rlm@7
|
38 [geom tri]
|
|
rlm@7
|
39 (let [new (Vector3f.)]
|
|
rlm@7
|
40 (.calculateCenter tri)
|
|
rlm@37
|
41 (.localToWorld geom (.getCenter tri) new) new))
|
|
rlm@6
|
42
|
|
rlm@7
|
43 (defn get-ray-direction
|
|
rlm@156
|
44 "Return the direction which a Ray would have to have to be to point
|
|
rlm@37
|
45 normal to the Triangle, in coordinates relative to the center of the
|
|
rlm@37
|
46 Triangle."
|
|
rlm@7
|
47 [geom tri]
|
|
rlm@9
|
48 (let [n+c (Vector3f.)]
|
|
rlm@7
|
49 (.calculateNormal tri)
|
|
rlm@9
|
50 (.calculateCenter tri)
|
|
rlm@37
|
51 (.localToWorld
|
|
rlm@37
|
52 geom
|
|
rlm@37
|
53 (.add (.getCenter tri) (.getNormal tri)) n+c)
|
|
rlm@37
|
54 (.subtract n+c (get-ray-origin geom tri))))
|
|
rlm@37
|
55
|
|
rlm@156
|
56 ;; Every Mesh has many triangles, each with its own index.
|
|
rlm@156
|
57 ;; Every vertex has its own index as well.
|
|
rlm@37
|
58
|
|
rlm@178
|
59 (defn tactile-sensor-profile
|
|
rlm@156
|
60 "Return the touch-sensor distribution image in BufferedImage format,
|
|
rlm@156
|
61 or nil if it does not exist."
|
|
rlm@156
|
62 [#^Geometry obj]
|
|
rlm@156
|
63 (if-let [image-path (meta-data obj "touch")]
|
|
rlm@178
|
64 (load-image image-path)))
|
|
rlm@156
|
65
|
|
rlm@156
|
66 (defn triangle
|
|
rlm@156
|
67 "Get the triangle specified by triangle-index from the mesh within
|
|
rlm@156
|
68 bounds."
|
|
rlm@156
|
69 [#^Mesh mesh triangle-index]
|
|
rlm@156
|
70 (let [scratch (Triangle.)]
|
|
rlm@156
|
71 (.getTriangle mesh triangle-index scratch)
|
|
rlm@156
|
72 scratch))
|
|
rlm@156
|
73
|
|
rlm@156
|
74 (defn triangle-vertex-indices
|
|
rlm@156
|
75 "Get the triangle vertex indices of a given triangle from a given
|
|
rlm@156
|
76 mesh."
|
|
rlm@156
|
77 [#^Mesh mesh triangle-index]
|
|
rlm@156
|
78 (let [indices (int-array 3)]
|
|
rlm@156
|
79 (.getTriangle mesh triangle-index indices)
|
|
rlm@156
|
80 (vec indices)))
|
|
rlm@156
|
81
|
|
rlm@156
|
82 (defn vertex-UV-coord
|
|
rlm@156
|
83 "Get the uv-coordinates of the vertex named by vertex-index"
|
|
rlm@156
|
84 [#^Mesh mesh vertex-index]
|
|
rlm@156
|
85 (let [UV-buffer
|
|
rlm@156
|
86 (.getData
|
|
rlm@156
|
87 (.getBuffer
|
|
rlm@156
|
88 mesh
|
|
rlm@156
|
89 VertexBuffer$Type/TexCoord))]
|
|
rlm@156
|
90 [(.get UV-buffer (* vertex-index 2))
|
|
rlm@156
|
91 (.get UV-buffer (+ 1 (* vertex-index 2)))]))
|
|
rlm@156
|
92
|
|
rlm@156
|
93 (defn triangle-UV-coord
|
|
rlm@156
|
94 "Get the uv-cooridnates of the triangle's verticies."
|
|
rlm@156
|
95 [#^Mesh mesh width height triangle-index]
|
|
rlm@156
|
96 (map (fn [[u v]] (vector (* width u) (* height v)))
|
|
rlm@156
|
97 (map (partial vertex-UV-coord mesh)
|
|
rlm@156
|
98 (triangle-vertex-indices mesh triangle-index))))
|
|
rlm@156
|
99
|
|
rlm@156
|
100 (defn same-side?
|
|
rlm@156
|
101 "Given the points p1 and p2 and the reference point ref, is point p
|
|
rlm@156
|
102 on the same side of the line that goes through p1 and p2 as ref is?"
|
|
rlm@156
|
103 [p1 p2 ref p]
|
|
rlm@156
|
104 (<=
|
|
rlm@156
|
105 0
|
|
rlm@156
|
106 (.dot
|
|
rlm@156
|
107 (.cross (.subtract p2 p1) (.subtract p p1))
|
|
rlm@156
|
108 (.cross (.subtract p2 p1) (.subtract ref p1)))))
|
|
rlm@156
|
109
|
|
rlm@156
|
110 (defn triangle-seq [#^Triangle tri]
|
|
rlm@156
|
111 [(.get1 tri) (.get2 tri) (.get3 tri)])
|
|
rlm@156
|
112
|
|
rlm@156
|
113 (defn vector3f-seq [#^Vector3f v]
|
|
rlm@156
|
114 [(.getX v) (.getY v) (.getZ v)])
|
|
rlm@156
|
115
|
|
rlm@156
|
116 (defn inside-triangle?
|
|
rlm@156
|
117 "Is the point inside the triangle?"
|
|
rlm@156
|
118 {:author "Dylan Holmes"}
|
|
rlm@156
|
119 [#^Triangle tri #^Vector3f p]
|
|
rlm@156
|
120 (let [[vert-1 vert-2 vert-3] (triangle-seq tri)]
|
|
rlm@156
|
121 (and
|
|
rlm@156
|
122 (same-side? vert-1 vert-2 vert-3 p)
|
|
rlm@156
|
123 (same-side? vert-2 vert-3 vert-1 p)
|
|
rlm@156
|
124 (same-side? vert-3 vert-1 vert-2 p))))
|
|
rlm@156
|
125
|
|
rlm@156
|
126 (defn triangle->matrix4f
|
|
rlm@156
|
127 "Converts the triangle into a 4x4 matrix: The first three columns
|
|
rlm@156
|
128 contain the vertices of the triangle; the last contains the unit
|
|
rlm@156
|
129 normal of the triangle. The bottom row is filled with 1s."
|
|
rlm@156
|
130 [#^Triangle t]
|
|
rlm@156
|
131 (let [mat (Matrix4f.)
|
|
rlm@156
|
132 [vert-1 vert-2 vert-3]
|
|
rlm@156
|
133 ((comp vec map) #(.get t %) (range 3))
|
|
rlm@156
|
134 unit-normal (do (.calculateNormal t)(.getNormal t))
|
|
rlm@156
|
135 vertices [vert-1 vert-2 vert-3 unit-normal]]
|
|
rlm@156
|
136 (dorun
|
|
rlm@156
|
137 (for [row (range 4) col (range 3)]
|
|
rlm@37
|
138 (do
|
|
rlm@156
|
139 (.set mat col row (.get (vertices row)col))
|
|
rlm@156
|
140 (.set mat 3 row 1))))
|
|
rlm@156
|
141 mat))
|
|
rlm@156
|
142
|
|
rlm@156
|
143 (defn triangle-transformation
|
|
rlm@156
|
144 "Returns the affine transformation that converts each vertex in the
|
|
rlm@156
|
145 first triangle into the corresponding vertex in the second
|
|
rlm@156
|
146 triangle."
|
|
rlm@156
|
147 [#^Triangle tri-1 #^Triangle tri-2]
|
|
rlm@156
|
148 (.mult
|
|
rlm@156
|
149 (triangle->matrix4f tri-2)
|
|
rlm@156
|
150 (.invert (triangle->matrix4f tri-1))))
|
|
rlm@156
|
151
|
|
rlm@156
|
152 (defn point->vector2f [[u v]]
|
|
rlm@156
|
153 (Vector2f. u v))
|
|
rlm@156
|
154
|
|
rlm@156
|
155 (defn vector2f->vector3f [v]
|
|
rlm@156
|
156 (Vector3f. (.getX v) (.getY v) 0))
|
|
rlm@156
|
157
|
|
rlm@156
|
158 (defn map-triangle [f #^Triangle tri]
|
|
rlm@156
|
159 (Triangle.
|
|
rlm@156
|
160 (f 0 (.get1 tri))
|
|
rlm@156
|
161 (f 1 (.get2 tri))
|
|
rlm@156
|
162 (f 2 (.get3 tri))))
|
|
rlm@156
|
163
|
|
rlm@156
|
164 (defn points->triangle
|
|
rlm@156
|
165 "Convert a list of points into a triangle."
|
|
rlm@156
|
166 [points]
|
|
rlm@156
|
167 (apply #(Triangle. %1 %2 %3)
|
|
rlm@156
|
168 (map (fn [point]
|
|
rlm@156
|
169 (let [point (vec point)]
|
|
rlm@156
|
170 (Vector3f. (get point 0 0)
|
|
rlm@156
|
171 (get point 1 0)
|
|
rlm@156
|
172 (get point 2 0))))
|
|
rlm@156
|
173 (take 3 points))))
|
|
rlm@156
|
174
|
|
rlm@156
|
175 (defn convex-bounds
|
|
rlm@178
|
176 "Returns the smallest square containing the given vertices, as a
|
|
rlm@178
|
177 vector of integers [left top width height]."
|
|
rlm@156
|
178 [uv-verts]
|
|
rlm@156
|
179 (let [xs (map first uv-verts)
|
|
rlm@156
|
180 ys (map second uv-verts)
|
|
rlm@156
|
181 x0 (Math/floor (apply min xs))
|
|
rlm@156
|
182 y0 (Math/floor (apply min ys))
|
|
rlm@156
|
183 x1 (Math/ceil (apply max xs))
|
|
rlm@156
|
184 y1 (Math/ceil (apply max ys))]
|
|
rlm@156
|
185 [x0 y0 (- x1 x0) (- y1 y0)]))
|
|
rlm@156
|
186
|
|
rlm@156
|
187 (defn sensors-in-triangle
|
|
rlm@178
|
188 "Locate the touch sensors in the triangle, returning a map of their
|
|
rlm@178
|
189 UV and geometry-relative coordinates."
|
|
rlm@156
|
190 [image mesh tri-index]
|
|
rlm@156
|
191 (let [width (.getWidth image)
|
|
rlm@156
|
192 height (.getHeight image)
|
|
rlm@156
|
193 UV-vertex-coords (triangle-UV-coord mesh width height tri-index)
|
|
rlm@156
|
194 bounds (convex-bounds UV-vertex-coords)
|
|
rlm@156
|
195
|
|
rlm@156
|
196 cutout-triangle (points->triangle UV-vertex-coords)
|
|
rlm@156
|
197 UV-sensor-coords
|
|
rlm@156
|
198 (filter (comp (partial inside-triangle? cutout-triangle)
|
|
rlm@156
|
199 (fn [[u v]] (Vector3f. u v 0)))
|
|
rlm@156
|
200 (white-coordinates image bounds))
|
|
rlm@156
|
201 UV->geometry (triangle-transformation
|
|
rlm@156
|
202 cutout-triangle
|
|
rlm@156
|
203 (triangle mesh tri-index))
|
|
rlm@156
|
204 geometry-sensor-coords
|
|
rlm@156
|
205 (map (fn [[u v]] (.mult UV->geometry (Vector3f. u v 0)))
|
|
rlm@156
|
206 UV-sensor-coords)]
|
|
rlm@156
|
207 {:UV UV-sensor-coords :geometry geometry-sensor-coords}))
|
|
rlm@156
|
208
|
|
rlm@156
|
209 (defn-memo locate-feelers
|
|
rlm@178
|
210 "Search the geometry's tactile UV profile for touch sensors,
|
|
rlm@178
|
211 returning their positions in geometry-relative coordinates."
|
|
rlm@156
|
212 [#^Geometry geo]
|
|
rlm@156
|
213 (let [mesh (.getMesh geo)
|
|
rlm@156
|
214 num-triangles (.getTriangleCount mesh)]
|
|
rlm@178
|
215 (if-let [image (tactile-sensor-profile geo)]
|
|
rlm@156
|
216 (map
|
|
rlm@156
|
217 (partial sensors-in-triangle image mesh)
|
|
rlm@156
|
218 (range num-triangles))
|
|
rlm@156
|
219 (repeat (.getTriangleCount mesh) {:UV nil :geometry nil}))))
|
|
rlm@156
|
220
|
|
rlm@178
|
221 (defn-memo touch-topology
|
|
rlm@178
|
222 "Return a sequence of vectors of the form [x y] describing the
|
|
rlm@178
|
223 \"topology\" of the tactile sensors. Points that are close together
|
|
rlm@178
|
224 in the touch-topology are generally close together in the simulation."
|
|
rlm@178
|
225 [#^Gemoetry geo]
|
|
rlm@156
|
226 (vec (collapse (reduce concat (map :UV (locate-feelers geo))))))
|
|
rlm@156
|
227
|
|
rlm@178
|
228 (defn-memo feeler-coordinates
|
|
rlm@178
|
229 "The location of the touch sensors in world-space coordinates."
|
|
rlm@178
|
230 [#^Geometry geo]
|
|
rlm@156
|
231 (vec (map :geometry (locate-feelers geo))))
|
|
rlm@156
|
232
|
|
rlm@178
|
233 (defn touch-fn
|
|
rlm@178
|
234 "Returns a function which returns tactile sensory data when called
|
|
rlm@178
|
235 inside a running simulation."
|
|
rlm@178
|
236 [#^Geometry geo]
|
|
rlm@156
|
237 (let [feeler-coords (feeler-coordinates geo)
|
|
rlm@156
|
238 tris (triangles geo)
|
|
rlm@156
|
239 limit 0.1
|
|
rlm@156
|
240 ;;results (CollisionResults.)
|
|
rlm@156
|
241 ]
|
|
rlm@156
|
242 (if (empty? (touch-topology geo))
|
|
rlm@156
|
243 nil
|
|
rlm@156
|
244 (fn [node]
|
|
rlm@156
|
245 (let [sensor-origins
|
|
rlm@156
|
246 (map
|
|
rlm@156
|
247 #(map (partial local-to-world geo) %)
|
|
rlm@156
|
248 feeler-coords)
|
|
rlm@156
|
249 triangle-normals
|
|
rlm@156
|
250 (map (partial get-ray-direction geo)
|
|
rlm@156
|
251 tris)
|
|
rlm@156
|
252 rays
|
|
rlm@156
|
253 (flatten
|
|
rlm@156
|
254 (map (fn [origins norm]
|
|
rlm@156
|
255 (map #(doto (Ray. % norm)
|
|
rlm@156
|
256 (.setLimit limit)) origins))
|
|
rlm@156
|
257 sensor-origins triangle-normals))]
|
|
rlm@156
|
258 (vector
|
|
rlm@156
|
259 (touch-topology geo)
|
|
rlm@156
|
260 (vec
|
|
rlm@156
|
261 (for [ray rays]
|
|
rlm@156
|
262 (do
|
|
rlm@156
|
263 (let [results (CollisionResults.)]
|
|
rlm@156
|
264 (.collideWith node ray results)
|
|
rlm@156
|
265 (let [touch-objects
|
|
rlm@156
|
266 (filter #(not (= geo (.getGeometry %)))
|
|
rlm@156
|
267 results)]
|
|
rlm@156
|
268 (- 255
|
|
rlm@156
|
269 (if (empty? touch-objects) 255
|
|
rlm@156
|
270 (rem
|
|
rlm@156
|
271 (int
|
|
rlm@156
|
272 (* 255 (/ (.getDistance
|
|
rlm@156
|
273 (first touch-objects)) limit)))
|
|
rlm@156
|
274 256))))))))))))))
|
|
rlm@156
|
275
|
|
rlm@178
|
276 (defn touch!
|
|
rlm@178
|
277 "Endow the creature with the sense of touch. Returns a sequence of
|
|
rlm@178
|
278 functions, one for each body part with a tactile-sensor-proile,
|
|
rlm@178
|
279 each of which when called returns sensory data for that body part."
|
|
rlm@178
|
280 [#^Node creature]
|
|
rlm@178
|
281 (filter
|
|
rlm@178
|
282 (comp not nil?)
|
|
rlm@178
|
283 (map touch-fn
|
|
rlm@178
|
284 (filter #(isa? (class %) Geometry)
|
|
rlm@178
|
285 (node-seq creature)))))
|
|
rlm@156
|
286
|
|
rlm@188
|
287 (defn view-touch
|
|
rlm@189
|
288 "Creates a function which accepts a list of touch sensor-data and
|
|
rlm@189
|
289 displays each element to the screen."
|
|
rlm@188
|
290 []
|
|
rlm@187
|
291 (view-sense
|
|
rlm@187
|
292 (fn
|
|
rlm@187
|
293 [[coords sensor-data]]
|
|
rlm@187
|
294 (let [image (points->image coords)]
|
|
rlm@187
|
295 (dorun
|
|
rlm@187
|
296 (for [i (range (count coords))]
|
|
rlm@187
|
297 (.setRGB image ((coords i) 0) ((coords i) 1)
|
|
rlm@187
|
298 (gray (sensor-data i)))))
|
|
rlm@188
|
299 image))))
|
|
rlm@37
|
300 #+end_src
|
|
rlm@37
|
301
|
|
rlm@226
|
302 * Headers
|
|
rlm@226
|
303 #+begin_src clojure
|
|
rlm@226
|
304 (ns cortex.touch
|
|
rlm@226
|
305 "Simulate the sense of touch in jMonkeyEngine3. Enables any Geometry
|
|
rlm@226
|
306 to be outfitted with touch sensors with density determined by a UV
|
|
rlm@226
|
307 image. In this way a Geometry can know what parts of itself are
|
|
rlm@226
|
308 touching nearby objects. Reads specially prepared blender files to
|
|
rlm@226
|
309 construct this sense automatically."
|
|
rlm@226
|
310 {:author "Robert McIntyre"}
|
|
rlm@226
|
311 (:use (cortex world util sense))
|
|
rlm@226
|
312 (:use clojure.contrib.def)
|
|
rlm@226
|
313 (:import (com.jme3.scene Geometry Node Mesh))
|
|
rlm@226
|
314 (:import com.jme3.collision.CollisionResults)
|
|
rlm@226
|
315 (:import com.jme3.scene.VertexBuffer$Type)
|
|
rlm@226
|
316 (:import (com.jme3.math Triangle Vector3f Vector2f Ray Matrix4f)))
|
|
rlm@226
|
317 #+end_src
|
|
rlm@37
|
318
|
|
rlm@226
|
319 * COMMENT Code Generation
|
|
rlm@39
|
320 #+begin_src clojure :tangle ../src/cortex/touch.clj
|
|
rlm@0
|
321 <<skin-main>>
|
|
rlm@0
|
322 #+end_src
|
|
rlm@0
|
323
|
|
rlm@68
|
324 #+begin_src clojure :tangle ../src/cortex/test/touch.clj
|
|
rlm@39
|
325 #+end_src
|
|
rlm@39
|
326
|
|
rlm@0
|
327
|
|
rlm@0
|
328
|
|
rlm@0
|
329
|
|
rlm@32
|
330
|
|
rlm@32
|
331
|
|
rlm@226
|
332
|
