Mercurial > cortex

comparison org/body.org @ 133:2ed7e60d3821

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
FINALLY got proprioception working
author Robert McIntyre <rlm@mit.edu>
date Wed, 01 Feb 2012 02:27:18 -0700
parents 3206d5e20bee
children ac350a0ac6b0
comparison
equal deleted inserted replaced
132:3206d5e20bee 133:2ed7e60d3821
15 com.jme3.math.Quaternion 15 com.jme3.math.Quaternion
16 com.jme3.math.Vector2f 16 com.jme3.math.Vector2f
17 com.jme3.math.Matrix3f 17 com.jme3.math.Matrix3f
18 com.jme3.bullet.control.RigidBodyControl)) 18 com.jme3.bullet.control.RigidBodyControl))
19 19
20 20 (comment
21 21 (defn joint-proprioception
22 (defn quaternion-decompose [#^Quaternion q]
23 (map
24 #(arc-between % (.rotate q %))
25 [Vector3f/UNIT_X
26 Vector3f/UNIT_Y
27 Vector3f/UNIT_Z]))
28
29 (defn any-orthogonal
30 "Generate an arbitray (but stable) orthogonal vector to a given
31 vector."
32 [vector]
33 (let [x (.getX vector)
34 y (.getY vector)
35 z (.getZ vector)]
36 (cond
37 (not= x (float 0)) (Vector3f. (- z) 0 x)
38 (not= y (float 0)) (Vector3f. 0 (- z) y)
39 (not= z (float 0)) (Vector3f. 0 (- z) y)
40 true Vector3f/ZERO)))
41
42 (defn project-quaternion
43 "From http://stackoverflow.com/questions/3684269/
44 component-of-a-quaternion-rotation-around-an-axis.
45
46 Determine the amount of rotation a quaternion will
47 cause about a given axis."
48 [#^Quaternion q #^Vector3f axis]
49 (let [basis-1 (any-orthogonal axis)
50 basis-2 (.cross axis basis-1)
51 rotated (.mult q basis-1)
52 alpha (.dot basis-1 (.project rotated basis-1))
53 beta (.dot basis-2 (.project rotated basis-2))]
54 (Math/atan2 beta alpha)))
55
56 (defn right-handed? [vec1 vec2 vec3]
57 (< 0 (.dot (.cross vec1 vec2) vec3)))
58
59 (defn project-quaternion
60 "From http://stackoverflow.com/questions/3684269/
61 component-of-a-quaternion-rotation-around-an-axis.
62
63 Determine the amount of rotation a quaternion will
64 cause about a given axis."
65 [#^Quaternion q #^Vector3f axis]
66 (let [axis (.normalize axis)
67 basis-1 (any-orthogonal axis)
68 basis-2 (.cross axis basis-1)
69 rotated (.mult q basis-1)
70 rotated-in-plane (.add (.project rotated basis-1)
71 (.project rotated basis-2))]
72
73 ;; be sure to get sign from cross product
74 (if (right-handed? basis-1 rotated-in-plane axis)
75 (.angleBetween rotated-in-plane basis-1)
76 (- (* 2 Math/PI) (.angleBetween rotated-in-plane basis-1)))))
77
78
79
80 (defn joint-proprioception
81 "Relative position information for a two-part system connected by a 22 "Relative position information for a two-part system connected by a
82 joint. Gives the pitch, yaw, and roll of the 'B' object relative to 23 joint. Gives the pitch, yaw, and roll of the 'B' object relative to
83 the 'A' object, as determined by the joint." 24 the 'A' object, as determined by the joint."
84 [joint] 25 [joint]
85 (let [object-a (.getUserObject (.getBodyA joint)) 26 (let [object-a (.getUserObject (.getBodyA joint))
118 (doto (Quaternion.) 59 (doto (Quaternion.)
119 (.fromRotationMatrix rotate-a))) 60 (.fromRotationMatrix rotate-a)))
120 arm-b)] 61 arm-b)]
121 ;;(println-repl (.getName object-a) (.getName object-b)) 62 ;;(println-repl (.getName object-a) (.getName object-b))
122 [pitch yaw roll])) 63 [pitch yaw roll]))
123 64 )
124 65
125 66 (defn any-orthogonal
126 67 "Generate an arbitray (but stable) orthogonal vector to a given
127 (defn absolute-angle-between 68 vector."
128 [vec-1 vec-2] 69 [vector]
70 (let [x (.getX vector)
71 y (.getY vector)
72 z (.getZ vector)]
73 (cond
74 (not= x (float 0)) (Vector3f. (- z) 0 x)
75 (not= y (float 0)) (Vector3f. 0 (- z) y)
76 (not= z (float 0)) (Vector3f. 0 (- z) y)
77 true Vector3f/ZERO)))
78
79 (comment
80 (defn project-quaternion
81 "From http://stackoverflow.com/questions/3684269/
82 component-of-a-quaternion-rotation-around-an-axis.
83
84 Determine the amount of rotation a quaternion will
85 cause about a given axis."
86 [#^Quaternion q #^Vector3f axis]
87 (let [basis-1 (any-orthogonal axis)
88 basis-2 (.cross axis basis-1)
89 rotated (.mult q basis-1)
90 alpha (.dot basis-1 (.project rotated basis-1))
91 beta (.dot basis-2 (.project rotated basis-2))]
92 (Math/atan2 beta alpha)))
93 )
94
95 (defn right-handed? [vec1 vec2 vec3]
96 (< 0 (.dot (.cross vec1 vec2) vec3)))
97
98 (defn absolute-angle [vec1 vec2 axis]
99 (let [angle (.angleBetween vec1 vec2)]
100 (if (right-handed? vec1 vec2 axis)
101 angle (- (* 2 Math/PI) angle))))
102
103 (defn angle-min [& angles]
104 (first
105 (sort-by
106 (fn [angle]
107 (let [in-circle (Math/abs (rem angle (* 2 Math/PI)))]
108 (min in-circle
109 (- (* Math/PI 2) in-circle))))
110 angles)))
111
112 (defn project-quaternion
113 "From http://stackoverflow.com/questions/3684269/
114 component-of-a-quaternion-rotation-around-an-axis.
115
116 Determine the amount of rotation a quaternion will
117 cause about a given axis."
118 [#^Quaternion q #^Vector3f axis]
119 (let [axis (.normalize axis)
120 basis-1 (.normalize (any-orthogonal axis))
121 basis-2 (.cross axis basis-1)
122 rotated-1 (.mult q basis-1)
123 basis-1* (.normalize
124 (.add (.project rotated-1 basis-1)
125 (.project rotated-1 basis-2)))
126 rotated-2 (.mult q basis-2)
127 basis-2* (.normalize
128 (.add (.project rotated-2 basis-1)
129 (.project rotated-2 basis-2)))
130 angle-1
131 (absolute-angle basis-1 basis-1* axis)
132 angle-2
133 (absolute-angle basis-2 basis-2* axis)
134
135
136 angle (angle-min angle-1 angle-2)
137 ]
129 138
130 139
131 140 ;; be sure to get sign from cross product
141 (if false
142 (do
143 (println-repl "axis" axis)
144 (println-repl "basis-1" basis-1)
145 (println-repl "basis-2" basis-2)
146 (println-repl "rotated-1" rotated-1)
147 (println-repl "rotated-2" rotated-2)
148 (println-repl "basis-1*" basis-1*)
149 (println-repl "basis-2*" basis-2*)
150 (println-repl "angle-1" angle-1)
151 (println-repl "angle-2" angle-2)
152
153 (println-repl "angle" angle)
154 (println-repl "")))
155 angle))
156
157
158 (import com.jme3.scene.Node)
159
160 (defn joint-proprioception [#^Node parts #^Node joint]
161 (let [[obj-a obj-b] (cortex.silly/joint-targets parts joint)
162 joint-rot (.getWorldRotation joint)
163 x (.mult joint-rot Vector3f/UNIT_X)
164 y (.mult joint-rot Vector3f/UNIT_Y)
165 z (.mult joint-rot Vector3f/UNIT_Z)]
166 ;; this function will report proprioceptive information for the
167 ;; joint
168 (fn []
169 ;; x is the "twist" axis, y and z are the "bend" axes
170 (let [rot-a (.getWorldRotation obj-a)
171 rot-b (.getWorldRotation obj-b)
172 relative (.mult (.inverse rot-a) rot-b)
173 basis (doto (Matrix3f.)
174 (.setColumn 0 x)
175 (.setColumn 1 y)
176 (.setColumn 2 z))
177 rotation-about-joint
178 (doto (Quaternion.)
179 (.fromRotationMatrix
180 (.mult (.invert basis)
181 (.toRotationMatrix relative))))
182 [yaw roll pitch]
183 (seq (.toAngles rotation-about-joint nil))]
184 ;;return euler angles of the quaternion around the new basis
185 ;;[yaw pitch roll]
186 [yaw roll pitch]
187 ))))
188
189
190 (comment
132 191
133 (defn joint-proprioception 192 (defn joint-proprioception
134 [joint] 193 [joint]
135 (let [object-a (.getUserObject (.getBodyA joint)) 194 (let [object-a (.getUserObject (.getBodyA joint))
136 object-b (.getUserObject (.getBodyB joint)) 195 object-b (.getUserObject (.getBodyB joint))
137 196 rot-a (.clone (.getWorldRotation object-a))
138 arm-a 197 rot-b (.clone (.getWorldRotation object-b))
139 (.normalize
140 (.subtract
141 (.localToWorld object-a (.getPivotA joint) nil)
142 (.getWorldTranslation object-a)))
143 arm-b
144 (.normalize
145 (.subtract
146 (.localToWorld object-b (.getPivotB joint) nil)
147 (.getWorldTranslation object-b)))
148
149 rotate-a (.clone (.getWorldRotation object-a))
150 unrotate-a (.inverse (.getWorldRotation object-a))
151
152 canonical-arm-a (.mult unrotate-a arm-a)
153
154 basis-1 (any-orthogonal canonical-arm-a)
155 basis-2 (.normalize (.cross basis-1 canonical-arm-a))
156
157 pitch (.angleBetween arm-b basis-1)
158 yaw (.angleBetween arm-b basis-2)
159
160 twist-a
161 (project-quaternion
162 (.getWorldRotation object-a) arm-a)
163
164 twist-b
165 (project-quaternion
166 (.getWorldRotation object-b) arm-b)
167
168 roll (- twist-b 0)
169 ;; "un-rotate" arm-a to it's canonical position, get two
170 ;; orthogonal basis vectors. Rotate those two vectors back to
171 ;; the correct position get the rotations between them.
172
173 ;; twist is the rotation about arm-a of obj-b minus the
174 ;; rotation about arm-a of obj-a
175 ] 198 ]
199
200 (.mult rot-b (.inverse rot-a))
201
176 ;; object-a == hand 202 ;; object-a == hand
177 ;; object-b == finger 203 ;; object-b == finger
178 [pitch yaw roll])) 204 ))
179 205 )
180 ;; (defn joint-proprioception* 206 ;; (defn joint-proprioception*
181 ;; [joint] 207 ;; [joint]
182 ;; (let [object-a (.getUserObject (.getBodyA joint)) 208 ;; (let [object-a (.getUserObject (.getBodyA joint))
183 ;; object-b (.getUserObject (.getBodyB joint)) 209 ;; object-b (.getUserObject (.getBodyB joint))
184 210
208 (node-seq body)))))] 234 (node-seq body)))))]
209 (fn [] 235 (fn []
210 (map joint-proprioception joints)))) 236 (map joint-proprioception joints))))
211 237
212 #+end_src 238 #+end_src
239
240 #+results: proprioception
241 : #'cortex.body/proprioception
213 242
214 * Motor Control 243 * Motor Control
215 #+name: motor-control 244 #+name: motor-control
216 #+begin_src clojure 245 #+begin_src clojure
217 (in-ns 'cortex.body) 246 (in-ns 'cortex.body)
344 (cortex.silly/world-to-local obj-a world-pivot) 373 (cortex.silly/world-to-local obj-a world-pivot)
345 (cortex.silly/world-to-local obj-b world-pivot) 374 (cortex.silly/world-to-local obj-b world-pivot)
346 nil 375 nil
347 )) 376 ))
348 377
349 378 (import com.jme3.bullet.collision.PhysicsCollisionObject)
350 379
351 (defn blab-* [] 380 (defn blab-* []
352 (let [hand (box 0.5 0.2 0.2 :position (Vector3f. 0 0 0) 381 (let [hand (box 0.5 0.2 0.2 :position (Vector3f. 0 0 0)
353 :mass 0 :color ColorRGBA/Green) 382 :mass 0 :color ColorRGBA/Green)
354 finger (box 0.5 0.2 0.2 :position (Vector3f. 2.4 0 0) 383 finger (box 0.5 0.2 0.2 :position (Vector3f. 2.4 0 0)
395 image)))) 424 image))))
396 425
397 (defn proprioception-debug-window 426 (defn proprioception-debug-window
398 [] 427 []
399 (let [yaw (view-angle 0xFF0000) 428 (let [yaw (view-angle 0xFF0000)
400 pitch (view-angle 0x00FF00) 429 roll (view-angle 0x00FF00)
401 roll (view-angle 0xFFFFFF) 430 pitch (view-angle 0xFFFFFF)
402 v-yaw (view-image) 431 v-yaw (view-image)
432 v-roll (view-image)
403 v-pitch (view-image) 433 v-pitch (view-image)
404 v-roll (view-image)
405 ] 434 ]
406 (fn [prop-data] 435 (fn [prop-data]
407 (dorun 436 (dorun
408 (map 437 (map
409 (fn [[p y r]] 438 (fn [[y r p]]
410 (v-yaw (yaw y)) 439 (v-yaw (yaw y))
411 (v-roll (roll r)) 440 (v-roll (roll r))
412 (v-pitch (pitch p))) 441 (v-pitch (pitch p)))
413 prop-data))))) 442 prop-data)))))
414 443 (comment
444
445 (defn proprioception-debug-window
446 []
447 (let [time (atom 0)]
448 (fn [prop-data]
449 (if (= 0 (rem (swap! time inc) 40))
450 (println-repl prop-data)))))
451 )
452
415 (comment 453 (comment
416 (dorun 454 (dorun
417 (map 455 (map
418 (comp 456 (comp
419 println-repl 457 println-repl
437 [] 475 []
438 (let [hand (box 1 0.2 0.2 :position (Vector3f. 0 2 0) 476 (let [hand (box 1 0.2 0.2 :position (Vector3f. 0 2 0)
439 :mass 0 :color ColorRGBA/Green :name "hand") 477 :mass 0 :color ColorRGBA/Green :name "hand")
440 finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0) 478 finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0)
441 :mass 1 :color ColorRGBA/Red :name "finger") 479 :mass 1 :color ColorRGBA/Red :name "finger")
442 floor (box 10 0.5 10 :position (Vector3f. 0 -5 0) 480 floor (box 10 10 10 :position (Vector3f. 0 -15 0)
443 :mass 0 :color ColorRGBA/Gray) 481 :mass 0 :color ColorRGBA/Gray)
482 joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow
483 :position (Vector3f. 1.2 2 0)
484 :physical? false)
444 485
445 move-up? (atom false) 486 move-up? (atom false)
446 move-down? (atom false) 487 move-down? (atom false)
447 move-left? (atom false) 488 move-left? (atom false)
448 move-right? (atom false) 489 move-right? (atom false)
449 roll-left? (atom false) 490 roll-left? (atom false)
450 roll-right? (atom false) 491 roll-right? (atom false)
451 control (.getControl finger RigidBodyControl) 492 control (.getControl finger RigidBodyControl)
452 time (atom 0) 493 time (atom 0)
453 joint (join-at-point hand finger (Vector3f. 1.2 2 0 )) 494 joint (join-at-point hand finger (Vector3f. 1.2 2 0 ))
454 creature (nodify [hand finger]) 495 creature (nodify [hand finger joint-node])
455 prop (proprioception creature) 496 prop (joint-proprioception creature joint-node)
456 497
457 prop-view (proprioception-debug-window) 498 prop-view (proprioception-debug-window)
458 499
459 500
460 ] 501 ]
461 502
462 503
463 504
505
464 (.setCollisionGroup 506 (.setCollisionGroup
465 (.getControl hand RigidBodyControl) 507 (.getControl hand RigidBodyControl)
466 PhysicsCollisionObject/COLLISION_GROUP_NONE) 508 PhysicsCollisionObject/COLLISION_GROUP_NONE)
467 509
468 510
469 (world 511 (world
470 (nodify [hand finger floor]) 512 (nodify [hand finger floor joint-node])
471 (merge standard-debug-controls 513 (merge standard-debug-controls
472 {"key-r" (fn [_ pressed?] (reset! move-up? pressed?)) 514 {"key-r" (fn [_ pressed?] (reset! move-up? pressed?))
473 "key-t" (fn [_ pressed?] (reset! move-down? pressed?)) 515 "key-t" (fn [_ pressed?] (reset! move-down? pressed?))
474 "key-f" (fn [_ pressed?] (reset! move-left? pressed?)) 516 "key-f" (fn [_ pressed?] (reset! move-left? pressed?))
475 "key-g" (fn [_ pressed?] (reset! move-right? pressed?)) 517 "key-g" (fn [_ pressed?] (reset! move-right? pressed?))
504 (.applyTorque control 546 (.applyTorque control
505 (.mult (.getPhysicsRotation control) 547 (.mult (.getPhysicsRotation control)
506 (Vector3f. 1 0 0)))) 548 (Vector3f. 1 0 0))))
507 549
508 ;;(if (= 0 (rem (swap! time inc) 20)) 550 ;;(if (= 0 (rem (swap! time inc) 20))
509 (prop-view (prop)))))) 551 (prop-view (list (prop)))))))
510 552
511 #+end_src 553 #+end_src
512 554
513 #+results: test-body 555 #+results: test-body
514 : #'cortex.test.body/test-proprioception 556 : #'cortex.test.body/test-proprioception