Mercurial > cortex

comparison org/body.org @ 134:ac350a0ac6b0

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
proprioception refrence frame is wrong, trying to fix...
author Robert McIntyre <rlm@mit.edu>
date Wed, 01 Feb 2012 02:44:07 -0700
parents 2ed7e60d3821
children 421cc43441ae
comparison
equal deleted inserted replaced
133:2ed7e60d3821 134:ac350a0ac6b0
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 (comment
21 (defn joint-proprioception
22 "Relative position information for a two-part system connected by a
23 joint. Gives the pitch, yaw, and roll of the 'B' object relative to
24 the 'A' object, as determined by the joint."
25 [joint]
26 (let [object-a (.getUserObject (.getBodyA joint))
27 object-b (.getUserObject (.getBodyB joint))
28 arm-a
29 (.normalize
30 (.subtract
31 (.localToWorld object-a (.getPivotA joint) nil)
32 (.getWorldTranslation object-a)))
33
34 ;; this is probably wrong!
35 rotate-a
36 (doto (Matrix3f.)
37 (.fromStartEndVectors arm-a Vector3f/UNIT_X))
38
39 arm-b
40 (.mult
41 rotate-a
42 (.normalize
43 (.subtract
44 (.localToWorld object-b (.getPivotB joint) nil)
45 (.getWorldTranslation object-b))))
46 pitch
47 (.angleBetween
48 (.normalize (Vector2f. (.getX arm-b) (.getY arm-b)))
49 (Vector2f. 1 0))
50 yaw
51 (.angleBetween
52 (.normalize (Vector2f. (.getX arm-b) (.getZ arm-b)))
53 (Vector2f. 1 0))
54
55 roll
56 (project-quaternion
57 (.mult
58 (.getLocalRotation object-b)
59 (doto (Quaternion.)
60 (.fromRotationMatrix rotate-a)))
61 arm-b)]
62 ;;(println-repl (.getName object-a) (.getName object-b))
63 [pitch yaw roll]))
64 )
65
66 (defn any-orthogonal
67 "Generate an arbitray (but stable) orthogonal vector to a given
68 vector."
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 ]
138
139
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 19
158 (import com.jme3.scene.Node) 20 (import com.jme3.scene.Node)
159 21
160 (defn joint-proprioception [#^Node parts #^Node joint] 22 (defn joint-proprioception [#^Node parts #^Node joint]
161 (let [[obj-a obj-b] (cortex.silly/joint-targets parts joint) 23 (let [[obj-a obj-b] (cortex.silly/joint-targets parts joint)
185 ;;[yaw pitch roll] 47 ;;[yaw pitch roll]
186 [yaw roll pitch] 48 [yaw roll pitch]
187 )))) 49 ))))
188 50
189 51
190 (comment
191
192 (defn joint-proprioception
193 [joint]
194 (let [object-a (.getUserObject (.getBodyA joint))
195 object-b (.getUserObject (.getBodyB joint))
196 rot-a (.clone (.getWorldRotation object-a))
197 rot-b (.clone (.getWorldRotation object-b))
198 ]
199
200 (.mult rot-b (.inverse rot-a))
201
202 ;; object-a == hand
203 ;; object-b == finger
204 ))
205 )
206 ;; (defn joint-proprioception*
207 ;; [joint]
208 ;; (let [object-a (.getUserObject (.getBodyA joint))
209 ;; object-b (.getUserObject (.getBodyB joint))
210
211 ;; rotate-a (.clone (.getWorldRotation object-a))
212 ;; rotate-b (.clone (.getWorldRotation object-b))
213
214 ;; rotate-rel (.mult rotate-b (.inverse rotate-a))
215 ;; ]
216 ;; ((comp vec map) (partial project-quaternion rotate-rel)
217 ;; [Vector3f/UNIT_X
218 ;; Vector3f/UNIT_Y
219 ;; Vector3f/UNIT_Z])))
220
221
222 (defn proprioception 52 (defn proprioception
223 "Create a function that provides proprioceptive information about an 53 "Create a function that provides proprioceptive information about an
224 entire body." 54 entire body."
225 [body] 55 [#^Node creature]
226 ;; extract the body's joints 56 ;; extract the body's joints
227 (let [joints 57 (let [joints (cortex.silly/creature-joints creature)
228 (distinct 58 senses (map (partial joint-proprioception creature) joints)]
229 (reduce
230 concat
231 (map #(.getJoints %)
232 (keep
233 #(.getControl % RigidBodyControl)
234 (node-seq body)))))]
235 (fn [] 59 (fn []
236 (map joint-proprioception joints)))) 60 (map #(%) senses))))
237 61
238 #+end_src 62 #+end_src
239 63
240 #+results: proprioception 64 #+results: proprioception
241 : #'cortex.body/proprioception 65 : #'cortex.body/proprioception