Mercurial > cortex

comparison org/body.org @ 131:e98850b83c2c

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
going to fix error in proprioception
author Robert McIntyre <rlm@mit.edu>
date Mon, 30 Jan 2012 07:21:46 -0700
parents b26017d1fe9a
children 3206d5e20bee
comparison
equal deleted inserted replaced
130:b26017d1fe9a 131:e98850b83c2c
54 arm-a 54 arm-a
55 (.normalize 55 (.normalize
56 (.subtract 56 (.subtract
57 (.localToWorld object-a (.getPivotA joint) nil) 57 (.localToWorld object-a (.getPivotA joint) nil)
58 (.getWorldTranslation object-a))) 58 (.getWorldTranslation object-a)))
59
60 ;; this is probably wrong!
59 rotate-a 61 rotate-a
60 (doto (Matrix3f.) 62 (doto (Matrix3f.)
61 (.fromStartEndVectors arm-a Vector3f/UNIT_X)) 63 (.fromStartEndVectors arm-a Vector3f/UNIT_X))
64
62 arm-b 65 arm-b
63 (.mult 66 (.mult
64 rotate-a 67 rotate-a
65 (.normalize 68 (.normalize
66 (.subtract 69 (.subtract
83 (.fromRotationMatrix rotate-a))) 86 (.fromRotationMatrix rotate-a)))
84 arm-b)] 87 arm-b)]
85 ;;(println-repl (.getName object-a) (.getName object-b)) 88 ;;(println-repl (.getName object-a) (.getName object-b))
86 [pitch yaw roll])) 89 [pitch yaw roll]))
87 90
91
92 (defn joint-proprioception
93 [joint]
94 (let [object-a (.getUserObject (.getBodyA joint))
95 object-b (.getUserObject (.getBodyB joint))
96
97 arm-a
98 (.normalize
99 (.subtract
100 (.localToWorld object-a (.getPivotA joint) nil)
101 (.getWorldTranslation object-a)))
102 arm-b
103 (.normalize
104 (.subtract
105 (.localToWorld object-b (.getPivotB joint) nil)
106 (.getWorldTranslation object-b)))
107
108
109 ]
110
111
112
113
114
88 (defn proprioception 115 (defn proprioception
89 "Create a function that provides proprioceptive information about an 116 "Create a function that provides proprioceptive information about an
90 entire body." 117 entire body."
91 [body] 118 [body]
92 ;; extract the body's joints 119 ;; extract the body's joints
260 (enable-debug world) 287 (enable-debug world)
261 (.setTimer world (com.aurellem.capture.RatchetTimer. 60)) 288 (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
262 (set-gravity world Vector3f/ZERO) 289 (set-gravity world Vector3f/ZERO)
263 ) 290 )
264 no-op))) 291 no-op)))
265 292 (import java.awt.image.BufferedImage)
293
294 (defn draw-sprite [image sprite x y color ]
295 (dorun
296 (for [[u v] sprite]
297 (.setRGB image (+ u x) (+ v y) color))))
298
299 (defn view-angle
300 "create a debug biew of an angle"
301 [color]
302 (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
303 previous (atom [25 25])
304 sprite [[0 0] [0 1]
305 [0 -1] [-1 0] [1 0]]]
306 (fn [angle]
307 (let [angle (float angle)]
308 (let [position
309 [(+ 25 (int (* 20 (Math/cos angle))))
310 (+ 25 (int (* 20(Math/sin angle))))]]
311 (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)
312 (draw-sprite image sprite (position 0) (position 1) color)
313 (reset! previous position))
314 image))))
315
266 (defn proprioception-debug-window 316 (defn proprioception-debug-window
267 [] 317 []
268 (let [vi (view-image)] 318 (let [yaw (view-angle 0xFF0000)
319 pitch (view-angle 0x00FF00)
320 roll (view-angle 0xFFFFFF)
321 v-yaw (view-image)
322 v-pitch (view-image)
323 v-roll (view-image)
324 ]
269 (fn [prop-data] 325 (fn [prop-data]
270 (dorun 326 (dorun
271 (map 327 (map
272 (comp
273 println-repl
274 (fn [[p y r]] 328 (fn [[p y r]]
275 (format 329 (v-yaw (yaw y))
276 "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n" 330 (v-roll (roll r))
277 p y r))) 331 (v-pitch (pitch p)))
278 prop-data))))) 332 prop-data)))))
279 333
334 (comment
335 (dorun
336 (map
337 (comp
338 println-repl
339 (fn [[p y r]]
340 (format
341 "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n"
342 p y r)))
343 prop-data)))
344
280 345
281 346
282 347
283 348
284 (defn test-proprioception 349 (defn test-proprioception
357 (if @roll-right? 422 (if @roll-right?
358 (.applyTorque control 423 (.applyTorque control
359 (.mult (.getPhysicsRotation control) 424 (.mult (.getPhysicsRotation control)
360 (Vector3f. 1 0 0)))) 425 (Vector3f. 1 0 0))))
361 426
362 (if (= 0 (rem (swap! time inc) 20)) 427 ;;(if (= 0 (rem (swap! time inc) 20))
363 (prop-view 428 (prop-view (prop))))))
364 (joint-proprioception joint))))))) 429
365
366
367 #+end_src 430 #+end_src
368 431
369 #+results: test-body 432 #+results: test-body
370 : #'cortex.test.body/test-proprioception 433 : #'cortex.test.body/test-proprioception
371 434