Mercurial > cortex
diff org/test-creature.org @ 133:2ed7e60d3821
FINALLY got proprioception working
author | Robert McIntyre <rlm@mit.edu> |
---|---|
date | Wed, 01 Feb 2012 02:27:18 -0700 |
parents | b26017d1fe9a |
children | ac350a0ac6b0 |
line wrap: on
line diff
1.1 --- a/org/test-creature.org Tue Jan 31 01:40:47 2012 -0700 1.2 +++ b/org/test-creature.org Wed Feb 01 02:27:18 2012 -0700 1.3 @@ -297,6 +297,41 @@ 1.4 (take 2 targets)) 1.5 (recur (float (* radius 2)))))))) 1.6 1.7 + 1.8 +(defn proprio-joint [#^Node parts #^Node joint] 1.9 + (let [[obj-a obj-b] (joint-targets parts joint) 1.10 + joint-rot (.getWorldRotation joint) 1.11 + x (.mult joint-rot Vector3f/UNIT_X) 1.12 + y (.mult joint-rot Vector3f/UNIT_Y) 1.13 + z (.mult joint-rot Vector3f/UNIT_Z)] 1.14 + ;; this function will report proprioceptive information for the 1.15 + ;; joint 1.16 + (fn [] 1.17 + ;; x is the "twist" axis, y and z are the "bend" axes 1.18 + (let [rot-a (.getWorldRotation obj-a) 1.19 + rot-b (.getWorldRotation obj-b) 1.20 + relative (.mult (.inverse rot-a) rot-b) 1.21 + basis (doto (Matrix3f.) 1.22 + (.setColumn 0 y) 1.23 + (.setColumn 1 z) 1.24 + (.setColumn 2 x)) 1.25 + rotation-about-joint 1.26 + (doto (Quaternion.) 1.27 + (.fromRotationMatrix 1.28 + (.mult (.inverse basis) 1.29 + (.toRotationMatrix relative)))) 1.30 + 1.31 + confirm-axes 1.32 + (let [temp-axes (make-array Vector3f 3)] 1.33 + (.toAxes rotation-about-joint temp-axes) 1.34 + (seq temp-axes)) 1.35 + euler-angles 1.36 + (seq (.toAngles rotation-about-joint nil))] 1.37 + ;;return euler angles of the quaternion around the new basis 1.38 + euler-angles)))) 1.39 + 1.40 + 1.41 + 1.42 (defn world-to-local 1.43 "Convert the world coordinates into coordinates relative to the 1.44 object (i.e. local coordinates), taking into account the rotation 1.45 @@ -457,8 +492,7 @@ 1.46 (dorun 1.47 (map 1.48 (fn [joint] 1.49 - (let [[obj-a obj-b] 1.50 - (joint-targets pieces joint)] 1.51 + (let [[obj-a obj-b] (joint-targets pieces joint)] 1.52 (connect obj-a obj-b joint))) 1.53 joints)) 1.54 pieces) 1.55 @@ -1188,7 +1222,7 @@ 1.56 (comp (view-image) BufferedImage!)) 1.57 1.58 (add-eye world (.getCamera world) no-op) 1.59 - 1.60 + ;;(set-gravity world (Vector3f. 0 0 0)) 1.61 ;;(com.aurellem.capture.Capture/captureVideo 1.62 ;; world (file-str "/home/r/proj/ai-videos/hand")) 1.63 ;;(.setTimer world (RatchetTimer. 60)) 1.64 @@ -1323,7 +1357,7 @@ 1.65 #+end_src 1.66 1.67 #+results: body-1 1.68 -: #'cortex.silly/test-creature 1.69 +: #'cortex.silly/follow-test 1.70 1.71 1.72 * COMMENT purgatory