Mercurial > cortex
comparison org/body.org @ 133:2ed7e60d3821
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 |