Mercurial > cortex

comparison org/body.org @ 145:7a49b81ca1bf

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
finally got proprioception working to my satisfaction
author Robert McIntyre <rlm@mit.edu>
date Thu, 02 Feb 2012 12:49:11 -0700
parents 48f9cba082eb
children e1232043656a
comparison
equal deleted inserted replaced
144:48f9cba082eb 145:7a49b81ca1bf
61 [#^Node creature] 61 [#^Node creature]
62 (if-let [joint-node (.getChild creature "joints")] 62 (if-let [joint-node (.getChild creature "joints")]
63 (seq (.getChildren joint-node)) 63 (seq (.getChildren joint-node))
64 (do (println-repl "could not find JOINTS node") []))) 64 (do (println-repl "could not find JOINTS node") [])))
65 65
66 (defn right-handed? [vec1 vec2 vec3]
67 (< 0 (.dot (.cross vec1 vec2) vec3)))
68
69 (defn absolute-angle [vec1 vec2 axis]
70 (let [angle (.angleBetween vec1 vec2)]
71 (if (right-handed? vec1 vec2 axis)
72 angle (- (* 2 Math/PI) angle))))
73
74
66 (defn joint-proprioception [#^Node parts #^Node joint] 75 (defn joint-proprioception [#^Node parts #^Node joint]
67 (let [[obj-a obj-b] (joint-targets parts joint) 76 (let [[obj-a obj-b] (joint-targets parts joint)
68 joint-rot (.getWorldRotation joint) 77 joint-rot (.getWorldRotation joint)
69 pre-inv-a (.inverse (.getWorldRotation obj-a)) 78 x0 (.mult joint-rot Vector3f/UNIT_X)
70 x (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_X)) 79 y0 (.mult joint-rot Vector3f/UNIT_Y)
71 y (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Y)) 80 z0 (.mult joint-rot Vector3f/UNIT_Z)]
72 z (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Z)) 81 (println-repl "x:" x0)
73 tmp-rot-a (.getWorldRotation obj-a)] 82 (println-repl "y:" y0)
74 (println-repl "x:" (.mult tmp-rot-a x)) 83 (println-repl "z:" z0)
75 (println-repl "y:" (.mult tmp-rot-a y)) 84 (println-repl "init-a:" (.getWorldRotation obj-a))
76 (println-repl "z:" (.mult tmp-rot-a z)) 85 (println-repl "init-b:" (.getWorldRotation obj-b))
77 (println-repl "rot-a" (.getWorldRotation obj-a)) 86
78 (println-repl "rot-b" (.getWorldRotation obj-b))
79 ;; this function will report proprioceptive information for the
80 ;; joint.
81 (fn [] 87 (fn []
82 ;; x is the "twist" axis, y and z are the "bend" axes 88 (let [rot-a (.clone (.getWorldRotation obj-a))
83 (let [rot-a (.getWorldRotation obj-a) 89 rot-b (.clone (.getWorldRotation obj-b))
84 ;;inv-a (.inverse rot-a) 90 x (.mult rot-a x0)
85 rot-b (.getWorldRotation obj-b) 91 y (.mult rot-a y0)
86 ;;relative (.mult rot-b inv-a) 92 z (.mult rot-a z0)
87 basis (doto (Matrix3f.) 93
88 (.setColumn 0 (.mult rot-a x)) 94 X (.mult rot-b x0)
89 (.setColumn 1 (.mult rot-a y)) 95 Y (.mult rot-b y0)
90 (.setColumn 2 (.mult rot-a z))) 96 Z (.mult rot-b z0)
91 rotation-about-joint 97 heading (Math/atan2 (.dot X z) (.dot X x))
98 pitch (Math/atan2 (.dot X y) (.dot X x))
99
100 ;; rotate x-vector back to origin
101 reverse
92 (doto (Quaternion.) 102 (doto (Quaternion.)
93 (.fromRotationMatrix 103 (.fromAngleAxis
94 (.mult (.invert basis) 104 (.angleBetween X x)
95 (.toRotationMatrix rot-b)))) 105 (let [cross (.normalize (.cross X x))]
96 [yaw roll pitch] 106 (if (= 0 (.length cross)) y cross))))
97 (seq (.toAngles rotation-about-joint nil))] 107 roll (absolute-angle (.mult reverse Y) y x)]
98 ;;return euler angles of the quaternion around the new basis 108
99 [yaw roll pitch] 109 [heading pitch roll]))))
100 ))))
101
102 110
103 (defn proprioception 111 (defn proprioception
104 "Create a function that provides proprioceptive information about an 112 "Create a function that provides proprioceptive information about an
105 entire body." 113 entire body."
106 [#^Node creature] 114 [#^Node creature]
159 atoms directions forces))) 167 atoms directions forces)))
160 168
161 world-loop* (fn [world tpf] 169 world-loop* (fn [world tpf]
162 (world-loop world tpf) 170 (world-loop world tpf)
163 (splice-loop))] 171 (splice-loop))]
164 172 [root-node
165 [root-node 173 keymap*
166 keymap* 174 intilization
167 intilization 175 world-loop*]))
168 world-loop*])) 176
169 177 (import java.awt.image.BufferedImage)
170 178
179 (defn draw-sprite [image sprite x y color ]
180 (dorun
181 (for [[u v] sprite]
182 (.setRGB image (+ u x) (+ v y) color))))
183
184 (defn view-angle
185 "create a debug view of an angle"
186 [color]
187 (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
188 previous (atom [25 25])
189 sprite [[0 0] [0 1]
190 [0 -1] [-1 0] [1 0]]]
191 (fn [angle]
192 (let [angle (float angle)]
193 (let [position
194 [(+ 25 (int (* 20 (Math/cos angle))))
195 (+ 25 (int (* -20 (Math/sin angle))))]]
196 (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)
197 (draw-sprite image sprite (position 0) (position 1) color)
198 (reset! previous position))
199 image))))
200
201 (defn proprioception-debug-window
202 []
203 (let [heading (view-angle 0xFF0000)
204 pitch (view-angle 0x00FF00)
205 roll (view-angle 0xFFFFFF)
206 v-heading (view-image)
207 v-pitch (view-image)
208 v-roll (view-image)
209 ]
210 (fn [prop-data]
211 (dorun
212 (map
213 (fn [[h p r]]
214 (v-heading (heading h))
215 (v-pitch (pitch p))
216 (v-roll (roll r)))
217 prop-data)))))
218
219
171 #+end_src 220 #+end_src
172 221
173 #+results: proprioception 222 #+results: proprioception
174 : #'cortex.body/proprioception 223 : #'cortex.body/proprioception-debug-window
175 224
176 * Motor Control 225 * Motor Control
177 #+name: motor-control 226 #+name: motor-control
178 #+begin_src clojure 227 #+begin_src clojure
179 (in-ns 'cortex.body) 228 (in-ns 'cortex.body)
208 (:import 257 (:import
209 com.jme3.math.Vector3f 258 com.jme3.math.Vector3f
210 com.jme3.math.ColorRGBA 259 com.jme3.math.ColorRGBA
211 com.jme3.bullet.joints.Point2PointJoint 260 com.jme3.bullet.joints.Point2PointJoint
212 com.jme3.bullet.control.RigidBodyControl 261 com.jme3.bullet.control.RigidBodyControl
213 com.jme3.system.NanoTimer)) 262 com.jme3.system.NanoTimer
263 com.jme3.math.Quaternion))
214 264
215 (defn worm-segments 265 (defn worm-segments
216 "Create multiple evenly spaced box segments. They're fabulous!" 266 "Create multiple evenly spaced box segments. They're fabulous!"
217 [segment-length num-segments interstitial-space radius] 267 [segment-length num-segments interstitial-space radius]
218 (letfn [(nth-segment 268 (letfn [(nth-segment
331 (enable-debug world) 381 (enable-debug world)
332 (.setTimer world (com.aurellem.capture.RatchetTimer. 60)) 382 (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
333 (set-gravity world Vector3f/ZERO) 383 (set-gravity world Vector3f/ZERO)
334 ) 384 )
335 no-op))) 385 no-op)))
336 (import java.awt.image.BufferedImage)
337
338 (defn draw-sprite [image sprite x y color ]
339 (dorun
340 (for [[u v] sprite]
341 (.setRGB image (+ u x) (+ v y) color))))
342
343 (defn view-angle
344 "create a debug view of an angle"
345 [color]
346 (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
347 previous (atom [25 25])
348 sprite [[0 0] [0 1]
349 [0 -1] [-1 0] [1 0]]]
350 (fn [angle]
351 (let [angle (float angle)]
352 (let [position
353 [(+ 25 (int (* 20 (Math/cos angle))))
354 (+ 25 (int (* 20(Math/sin angle))))]]
355 (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)
356 (draw-sprite image sprite (position 0) (position 1) color)
357 (reset! previous position))
358 image))))
359
360 (defn proprioception-debug-window
361 []
362 (let [yaw (view-angle 0xFF0000)
363 roll (view-angle 0x00FF00)
364 pitch (view-angle 0xFFFFFF)
365 v-yaw (view-image)
366 v-roll (view-image)
367 v-pitch (view-image)
368 ]
369 (fn [prop-data]
370 (dorun
371 (map
372 (fn [[y r p]]
373 (v-yaw (yaw y))
374 (v-roll (roll r))
375 (v-pitch (pitch p)))
376 prop-data)))))
377 (comment 386 (comment
378 387
379 (defn proprioception-debug-window 388 (defn proprioception-debug-window
380 [] 389 []
381 (let [time (atom 0)] 390 (let [time (atom 0)]
404 roll. Pressing key-r/key-t should move the blue bar up and down and 413 roll. Pressing key-r/key-t should move the blue bar up and down and
405 change only the value of pitch. key-f/key-g moves it side to side 414 change only the value of pitch. key-f/key-g moves it side to side
406 and changes yaw. key-v/key-b will spin the blue segment clockwise 415 and changes yaw. key-v/key-b will spin the blue segment clockwise
407 and counterclockwise, and only affect roll." 416 and counterclockwise, and only affect roll."
408 [] 417 []
409 (let [hand (box 1 0.2 0.2 :position (Vector3f. 0 2 0) 418 (let [hand (box 0.2 1 0.2 :position (Vector3f. 0 0 0)
410 :mass 0 :color ColorRGBA/Green :name "hand") 419 :mass 0 :color ColorRGBA/Green :name "hand")
411 finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0) 420 finger (box 0.2 1 0.2 :position (Vector3f. 0 2.4 0)
412 :mass 1 :color ColorRGBA/Red :name "finger") 421 :mass 1 :color ColorRGBA/Red :name "finger")
413 joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow 422 joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow
414 :position (Vector3f. 1.2 2 0) 423 :position (Vector3f. 0 1.2 0)
424 :rotation (doto (Quaternion.)
425 (.fromAngleAxis
426 (/ Math/PI 2)
427 (Vector3f. 0 0 1)))
415 :physical? false) 428 :physical? false)
416 joint (join-at-point hand finger (Vector3f. 1.2 2 0 )) 429 joint (join-at-point hand finger (Vector3f. 0 1.2 0 ))
417 creature (nodify [hand finger joint-node]) 430 creature (nodify [hand finger joint-node])
431 finger-control (.getControl finger RigidBodyControl)
432 hand-control (.getControl hand RigidBodyControl)]
433
434
435 (let
418 ;; ******************************************* 436 ;; *******************************************
419 437
420 floor (box 10 10 10 :position (Vector3f. 0 -15 0) 438 [floor (box 10 10 10 :position (Vector3f. 0 -15 0)
421 :mass 0 :color ColorRGBA/Gray) 439 :mass 0 :color ColorRGBA/Gray)
422 440
423 root (nodify [creature floor]) 441 root (nodify [creature floor])
424 prop (joint-proprioception creature joint-node) 442 prop (joint-proprioception creature joint-node)
425 prop-view (proprioception-debug-window) 443 prop-view (proprioception-debug-window)
426 finger-control (.getControl finger RigidBodyControl)
427 hand-control (.getControl hand RigidBodyControl)
428 444
429 controls 445 controls
430 (merge standard-debug-controls 446 (merge standard-debug-controls
431 {"key-o" 447 {"key-o"
432 (fn [_ _] (.setEnabled finger-control true)) 448 (fn [_ _] (.setEnabled finger-control true))
469 ["key-y" "key-u" "key-h" "key-j" "key-n" "key-m"] 485 ["key-y" "key-u" "key-h" "key-j" "key-n" "key-m"]
470 [10 10 10 10 1 1] 486 [10 10 10 10 1 1]
471 (with-movement 487 (with-movement
472 finger 488 finger
473 ["key-r" "key-t" "key-f" "key-g" "key-v" "key-b"] 489 ["key-r" "key-t" "key-f" "key-g" "key-v" "key-b"]
474 [10 10 10 10 1 1] 490 [1 1 10 10 10 10]
475 [root 491 [root
476 controls 492 controls
477 (fn [world] 493 (fn [world]
478 (.setTimer world (com.aurellem.capture.RatchetTimer. 60)) 494 (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
479 (set-gravity world (Vector3f. 0 0 0)) 495 (set-gravity world (Vector3f. 0 0 0))
480 (light-up-everything world)) 496 (light-up-everything world))
481 (fn [_ _] (prop-view (list (prop))))]))))) 497 (fn [_ _] (prop-view (list (prop))))]))))))
482 498
483 #+end_src 499 #+end_src
484 500
485 #+results: test-body 501 #+results: test-body
486 : #'cortex.test.body/test-proprioception 502 : #'cortex.test.body/test-proprioception
906 (fn [world] 922 (fn [world]
907 (set-gravity world Vector3f/ZERO) 923 (set-gravity world Vector3f/ZERO)
908 (light-up-everything world) 924 (light-up-everything world)
909 (.setTimer world (NanoTimer.))) 925 (.setTimer world (NanoTimer.)))
910 (fn [_ _] 926 (fn [_ _]
911 (dorun (motor-map [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0])))))) 927 (dorun (motor-map [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0
928 0]))))))
929
930 (defn joint-proprioception [#^Node parts #^Node joint]
931 (let [[obj-a obj-b] (joint-targets parts joint)
932 joint-rot (.getWorldRotation joint)
933 pre-inv-a (.inverse (.getWorldRotation obj-a))
934 x (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_X))
935 y (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Y))
936 z (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Z))
937
938 x Vector3f/UNIT_Y
939 y Vector3f/UNIT_Z
940 z Vector3f/UNIT_X
941
942
943 tmp-rot-a (.getWorldRotation obj-a)]
944 (println-repl "x:" (.mult tmp-rot-a x))
945 (println-repl "y:" (.mult tmp-rot-a y))
946 (println-repl "z:" (.mult tmp-rot-a z))
947 (println-repl "rot-a" (.getWorldRotation obj-a))
948 (println-repl "rot-b" (.getWorldRotation obj-b))
949 (println-repl "joint-rot" joint-rot)
950 ;; this function will report proprioceptive information for the
951 ;; joint.
952 (fn []
953 ;; x is the "twist" axis, y and z are the "bend" axes
954 (let [rot-a (.getWorldRotation obj-a)
955 ;;inv-a (.inverse rot-a)
956 rot-b (.getWorldRotation obj-b)
957 ;;relative (.mult rot-b inv-a)
958 basis (doto (Matrix3f.)
959 (.setColumn 0 (.mult rot-a x))
960 (.setColumn 1 (.mult rot-a y))
961 (.setColumn 2 (.mult rot-a z)))
962 rotation-about-joint
963 (doto (Quaternion.)
964 (.fromRotationMatrix
965 (.mult (.invert basis)
966 (.toRotationMatrix rot-b))))
967 [yaw roll pitch]
968 (seq (.toAngles rotation-about-joint nil))]
969 ;;return euler angles of the quaternion around the new basis
970 [yaw roll pitch]))))
971
912 #+end_src 972 #+end_src
913 973
914 974
915 975
916 976