Mercurial > cortex

comparison org/body.org @ 132:3206d5e20bee

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
still trying to fix proprioception
author Robert McIntyre <rlm@mit.edu>
date Tue, 31 Jan 2012 01:40:47 -0700
parents e98850b83c2c
children 2ed7e60d3821
comparison
equal deleted inserted replaced
131:e98850b83c2c 132:3206d5e20bee
14 com.jme3.math.Vector3f 14 com.jme3.math.Vector3f
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
20
21
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]))
19 28
20 (defn any-orthogonal 29 (defn any-orthogonal
21 "Generate an arbitray (but stable) orthogonal vector to a given 30 "Generate an arbitray (but stable) orthogonal vector to a given
22 vector." 31 vector."
23 [vector] 32 [vector]
41 basis-2 (.cross axis basis-1) 50 basis-2 (.cross axis basis-1)
42 rotated (.mult q basis-1) 51 rotated (.mult q basis-1)
43 alpha (.dot basis-1 (.project rotated basis-1)) 52 alpha (.dot basis-1 (.project rotated basis-1))
44 beta (.dot basis-2 (.project rotated basis-2))] 53 beta (.dot basis-2 (.project rotated basis-2))]
45 (Math/atan2 beta alpha))) 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
46 79
47 (defn joint-proprioception 80 (defn joint-proprioception
48 "Relative position information for a two-part system connected by a 81 "Relative position information for a two-part system connected by a
49 joint. Gives the pitch, yaw, and roll of the 'B' object relative to 82 joint. Gives the pitch, yaw, and roll of the 'B' object relative to
50 the 'A' object, as determined by the joint." 83 the 'A' object, as determined by the joint."
87 arm-b)] 120 arm-b)]
88 ;;(println-repl (.getName object-a) (.getName object-b)) 121 ;;(println-repl (.getName object-a) (.getName object-b))
89 [pitch yaw roll])) 122 [pitch yaw roll]))
90 123
91 124
125
126
127 (defn absolute-angle-between
128 [vec-1 vec-2]
129
130
131
132
92 (defn joint-proprioception 133 (defn joint-proprioception
93 [joint] 134 [joint]
94 (let [object-a (.getUserObject (.getBodyA joint)) 135 (let [object-a (.getUserObject (.getBodyA joint))
95 object-b (.getUserObject (.getBodyB joint)) 136 object-b (.getUserObject (.getBodyB joint))
96 137
102 arm-b 143 arm-b
103 (.normalize 144 (.normalize
104 (.subtract 145 (.subtract
105 (.localToWorld object-b (.getPivotB joint) nil) 146 (.localToWorld object-b (.getPivotB joint) nil)
106 (.getWorldTranslation object-b))) 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)
107 163
108 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
109 ] 175 ]
110 176 ;; object-a == hand
111 177 ;; object-b == finger
112 178 [pitch yaw roll]))
179
180 ;; (defn joint-proprioception*
181 ;; [joint]
182 ;; (let [object-a (.getUserObject (.getBodyA joint))
183 ;; object-b (.getUserObject (.getBodyB joint))
184
185 ;; rotate-a (.clone (.getWorldRotation object-a))
186 ;; rotate-b (.clone (.getWorldRotation object-b))
187
188 ;; rotate-rel (.mult rotate-b (.inverse rotate-a))
189 ;; ]
190 ;; ((comp vec map) (partial project-quaternion rotate-rel)
191 ;; [Vector3f/UNIT_X
192 ;; Vector3f/UNIT_Y
193 ;; Vector3f/UNIT_Z])))
113 194
114 195
115 (defn proprioception 196 (defn proprioception
116 "Create a function that provides proprioceptive information about an 197 "Create a function that provides proprioceptive information about an
117 entire body." 198 entire body."
295 (dorun 376 (dorun
296 (for [[u v] sprite] 377 (for [[u v] sprite]
297 (.setRGB image (+ u x) (+ v y) color)))) 378 (.setRGB image (+ u x) (+ v y) color))))
298 379
299 (defn view-angle 380 (defn view-angle
300 "create a debug biew of an angle" 381 "create a debug view of an angle"
301 [color] 382 [color]
302 (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB) 383 (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
303 previous (atom [25 25]) 384 previous (atom [25 25])
304 sprite [[0 0] [0 1] 385 sprite [[0 0] [0 1]
305 [0 -1] [-1 0] [1 0]]] 386 [0 -1] [-1 0] [1 0]]]
353 change only the value of pitch. key-f/key-g moves it side to side 434 change only the value of pitch. key-f/key-g moves it side to side
354 and changes yaw. key-v/key-b will spin the blue segment clockwise 435 and changes yaw. key-v/key-b will spin the blue segment clockwise
355 and counterclockwise, and only affect roll." 436 and counterclockwise, and only affect roll."
356 [] 437 []
357 (let [hand (box 1 0.2 0.2 :position (Vector3f. 0 2 0) 438 (let [hand (box 1 0.2 0.2 :position (Vector3f. 0 2 0)
358 :mass 0 :color ColorRGBA/Green) 439 :mass 0 :color ColorRGBA/Green :name "hand")
359 finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0) 440 finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0)
360 :mass 1 :color ColorRGBA/Red) 441 :mass 1 :color ColorRGBA/Red :name "finger")
361 floor (box 10 0.5 10 :position (Vector3f. 0 -5 0) 442 floor (box 10 0.5 10 :position (Vector3f. 0 -5 0)
362 :mass 0 :color ColorRGBA/Gray) 443 :mass 0 :color ColorRGBA/Gray)
363 444
364 move-up? (atom false) 445 move-up? (atom false)
365 move-down? (atom false) 446 move-down? (atom false)