1.1 --- a/org/body.org	Thu Feb 02 03:17:11 2012 -0700
     1.2 +++ b/org/body.org	Thu Feb 02 12:49:11 2012 -0700
     1.3 @@ -63,42 +63,50 @@
     1.4      (seq (.getChildren joint-node))
     1.5      (do (println-repl "could not find JOINTS node") [])))
     1.6  
     1.7 +(defn right-handed? [vec1 vec2 vec3]
     1.8 +  (< 0 (.dot (.cross vec1 vec2) vec3)))
     1.9 +
    1.10 +(defn absolute-angle [vec1 vec2 axis]
    1.11 +  (let [angle (.angleBetween vec1 vec2)]
    1.12 +    (if (right-handed? vec1 vec2 axis)
    1.13 +      angle (- (* 2 Math/PI) angle))))
    1.14 +
    1.15 +
    1.16  (defn joint-proprioception [#^Node parts #^Node joint]
    1.17    (let [[obj-a obj-b] (joint-targets parts joint)
    1.18          joint-rot (.getWorldRotation joint)
    1.19 -        pre-inv-a (.inverse (.getWorldRotation obj-a))
    1.20 -        x (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_X))
    1.21 -        y (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Y))
    1.22 -        z (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Z))
    1.23 -        tmp-rot-a (.getWorldRotation obj-a)]
    1.24 -    (println-repl "x:" (.mult tmp-rot-a x))
    1.25 -    (println-repl "y:" (.mult tmp-rot-a y))
    1.26 -    (println-repl "z:" (.mult tmp-rot-a z))
    1.27 -    (println-repl "rot-a" (.getWorldRotation obj-a))
    1.28 -    (println-repl "rot-b" (.getWorldRotation obj-b))
    1.29 -    ;; this function will report proprioceptive information for the
    1.30 -    ;; joint.
    1.31 +        x0 (.mult joint-rot Vector3f/UNIT_X)
    1.32 +        y0 (.mult joint-rot Vector3f/UNIT_Y)
    1.33 +        z0 (.mult joint-rot Vector3f/UNIT_Z)]
    1.34 +    (println-repl "x:" x0)
    1.35 +    (println-repl "y:" y0)
    1.36 +    (println-repl "z:" z0)
    1.37 +    (println-repl "init-a:" (.getWorldRotation obj-a))
    1.38 +    (println-repl "init-b:" (.getWorldRotation obj-b))
    1.39 +
    1.40      (fn []
    1.41 -      ;; x is the "twist" axis, y and z are the "bend" axes
    1.42 -      (let [rot-a (.getWorldRotation obj-a)
    1.43 -            ;;inv-a (.inverse rot-a)
    1.44 -            rot-b (.getWorldRotation obj-b)
    1.45 -            ;;relative (.mult rot-b inv-a)
    1.46 -            basis (doto (Matrix3f.)
    1.47 -                    (.setColumn 0 (.mult rot-a x))
    1.48 -                    (.setColumn 1 (.mult rot-a y))
    1.49 -                    (.setColumn 2 (.mult rot-a z)))
    1.50 -            rotation-about-joint
    1.51 +      (let [rot-a (.clone (.getWorldRotation obj-a))
    1.52 +            rot-b (.clone (.getWorldRotation obj-b))
    1.53 +            x (.mult rot-a x0)
    1.54 +            y (.mult rot-a y0)
    1.55 +            z (.mult rot-a z0)
    1.56 +
    1.57 +            X (.mult rot-b x0)
    1.58 +            Y (.mult rot-b y0)
    1.59 +            Z (.mult rot-b z0)
    1.60 +            heading  (Math/atan2 (.dot X z) (.dot X x))
    1.61 +            pitch  (Math/atan2 (.dot X y) (.dot X x))
    1.62 +
    1.63 +            ;; rotate x-vector back to origin
    1.64 +            reverse
    1.65              (doto (Quaternion.)
    1.66 -              (.fromRotationMatrix 
    1.67 -               (.mult (.invert basis)
    1.68 -                      (.toRotationMatrix rot-b))))
    1.69 -            [yaw roll pitch]
    1.70 -            (seq (.toAngles rotation-about-joint nil))]
    1.71 -        ;;return euler angles of the quaternion around the new basis            
    1.72 -        [yaw roll pitch]
    1.73 -        ))))
    1.74 -
    1.75 +              (.fromAngleAxis
    1.76 +               (.angleBetween X x)
    1.77 +               (let [cross (.normalize (.cross X x))]
    1.78 +                 (if (= 0 (.length cross)) y cross))))
    1.79 +            roll (absolute-angle (.mult reverse Y) y x)]
    1.80 +        
    1.81 +        [heading pitch roll]))))
    1.82  
    1.83  (defn proprioception
    1.84    "Create a function that provides proprioceptive information about an
    1.85 @@ -161,17 +169,58 @@
    1.86          world-loop* (fn [world tpf]
    1.87                         (world-loop world tpf)
    1.88                         (splice-loop))]
    1.89 +    [root-node
    1.90 +     keymap*
    1.91 +     intilization
    1.92 +     world-loop*]))
    1.93  
    1.94 -     [root-node
    1.95 -      keymap*
    1.96 -      intilization
    1.97 -      world-loop*]))
    1.98 +(import java.awt.image.BufferedImage)
    1.99  
   1.100 -  
   1.101 +(defn draw-sprite [image sprite x y color ]
   1.102 +  (dorun
   1.103 +   (for [[u v] sprite]
   1.104 +     (.setRGB image (+ u x) (+ v y) color))))
   1.105 +
   1.106 +(defn view-angle
   1.107 +  "create a debug view of an angle"
   1.108 +  [color]
   1.109 +  (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
   1.110 +        previous (atom [25 25])
   1.111 +        sprite [[0 0] [0 1]
   1.112 +                [0 -1] [-1 0] [1 0]]] 
   1.113 +    (fn [angle]
   1.114 +      (let [angle (float angle)]
   1.115 +        (let [position
   1.116 +              [(+ 25 (int (* 20 (Math/cos angle))))
   1.117 +               (+ 25 (int (* -20 (Math/sin angle))))]]
   1.118 +          (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)
   1.119 +          (draw-sprite image sprite (position 0) (position 1) color)
   1.120 +          (reset! previous position))
   1.121 +        image))))
   1.122 +
   1.123 +(defn proprioception-debug-window
   1.124 +  []
   1.125 +  (let [heading (view-angle 0xFF0000)
   1.126 +        pitch (view-angle 0x00FF00)
   1.127 +        roll (view-angle 0xFFFFFF)
   1.128 +        v-heading (view-image)
   1.129 +        v-pitch (view-image)
   1.130 +        v-roll (view-image)
   1.131 +        ]
   1.132 +    (fn [prop-data]
   1.133 +      (dorun
   1.134 +       (map
   1.135 +        (fn [[h p r]]
   1.136 +          (v-heading (heading h))
   1.137 +          (v-pitch (pitch p))
   1.138 +          (v-roll (roll r)))
   1.139 +        prop-data)))))
   1.140 +
   1.141 +
   1.142  #+end_src
   1.143  
   1.144  #+results: proprioception
   1.145 -: #'cortex.body/proprioception
   1.146 +: #'cortex.body/proprioception-debug-window
   1.147  
   1.148  * Motor Control
   1.149  #+name: motor-control
   1.150 @@ -210,7 +259,8 @@
   1.151     com.jme3.math.ColorRGBA
   1.152     com.jme3.bullet.joints.Point2PointJoint
   1.153     com.jme3.bullet.control.RigidBodyControl
   1.154 -   com.jme3.system.NanoTimer))
   1.155 +   com.jme3.system.NanoTimer
   1.156 +   com.jme3.math.Quaternion))
   1.157  
   1.158  (defn worm-segments
   1.159    "Create multiple evenly spaced box segments. They're fabulous!"
   1.160 @@ -333,47 +383,6 @@
   1.161         (set-gravity world Vector3f/ZERO)
   1.162         )
   1.163       no-op)))
   1.164 -(import java.awt.image.BufferedImage)
   1.165 -
   1.166 -(defn draw-sprite [image sprite x y color ]
   1.167 -  (dorun
   1.168 -   (for [[u v] sprite]
   1.169 -     (.setRGB image (+ u x) (+ v y) color))))
   1.170 -
   1.171 -(defn view-angle
   1.172 -  "create a debug view of an angle"
   1.173 -  [color]
   1.174 -  (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
   1.175 -        previous (atom [25 25])
   1.176 -        sprite [[0 0] [0 1]
   1.177 -                [0 -1] [-1 0] [1 0]]] 
   1.178 -    (fn [angle]
   1.179 -      (let [angle (float angle)]
   1.180 -        (let [position
   1.181 -              [(+ 25 (int (* 20 (Math/cos angle))))
   1.182 -               (+ 25 (int (* 20(Math/sin angle))))]]
   1.183 -          (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)
   1.184 -          (draw-sprite image sprite (position 0) (position 1) color)
   1.185 -          (reset! previous position))
   1.186 -        image))))
   1.187 -
   1.188 -(defn proprioception-debug-window
   1.189 -  []
   1.190 -  (let [yaw (view-angle 0xFF0000)
   1.191 -        roll (view-angle 0x00FF00)
   1.192 -        pitch (view-angle 0xFFFFFF)
   1.193 -        v-yaw (view-image)
   1.194 -        v-roll (view-image)
   1.195 -        v-pitch (view-image)
   1.196 -        ]
   1.197 -    (fn [prop-data]
   1.198 -      (dorun
   1.199 -       (map
   1.200 -        (fn [[y r p]]
   1.201 -          (v-yaw (yaw y))
   1.202 -          (v-roll (roll r))
   1.203 -          (v-pitch (pitch p)))
   1.204 -        prop-data)))))
   1.205  (comment  
   1.206       
   1.207  (defn proprioception-debug-window
   1.208 @@ -406,25 +415,32 @@
   1.209     and changes yaw. key-v/key-b will spin the blue segment clockwise
   1.210     and counterclockwise, and only affect roll."
   1.211    []
   1.212 -  (let [hand    (box 1 0.2 0.2 :position (Vector3f. 0 2 0)
   1.213 +  (let [hand    (box 0.2 1 0.2 :position (Vector3f. 0 0 0)
   1.214                       :mass 0 :color ColorRGBA/Green :name "hand")
   1.215 -        finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0)
   1.216 +        finger (box 0.2 1 0.2 :position (Vector3f. 0 2.4 0)
   1.217                      :mass 1 :color ColorRGBA/Red :name "finger")
   1.218          joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow
   1.219 -                        :position (Vector3f. 1.2 2 0)
   1.220 +                        :position (Vector3f. 0 1.2 0)
   1.221 +                        :rotation (doto (Quaternion.)
   1.222 +                                    (.fromAngleAxis
   1.223 +                                     (/ Math/PI 2)
   1.224 +                                     (Vector3f. 0 0 1)))
   1.225                          :physical? false)
   1.226 -        joint (join-at-point hand finger (Vector3f. 1.2 2 0 ))
   1.227 +        joint (join-at-point hand finger (Vector3f. 0 1.2 0 ))
   1.228          creature (nodify [hand finger joint-node])
   1.229 +        finger-control (.getControl finger RigidBodyControl)
   1.230 +        hand-control (.getControl hand RigidBodyControl)]
   1.231 +    
   1.232 +
   1.233 +    (let
   1.234          ;; *******************************************
   1.235                  
   1.236 -        floor   (box 10 10 10 :position (Vector3f. 0 -15 0)
   1.237 +        [floor   (box 10 10 10 :position (Vector3f. 0 -15 0)
   1.238                       :mass 0 :color ColorRGBA/Gray)
   1.239          
   1.240          root (nodify [creature floor])
   1.241          prop (joint-proprioception creature joint-node)
   1.242          prop-view (proprioception-debug-window)
   1.243 -        finger-control (.getControl finger RigidBodyControl)
   1.244 -        hand-control (.getControl hand RigidBodyControl)
   1.245          
   1.246          controls
   1.247          (merge standard-debug-controls
   1.248 @@ -471,14 +487,14 @@
   1.249         (with-movement
   1.250           finger
   1.251           ["key-r" "key-t" "key-f" "key-g" "key-v" "key-b"]
   1.252 -         [10 10 10 10 1 1]
   1.253 +         [1 1 10 10 10 10]
   1.254           [root
   1.255            controls
   1.256            (fn [world]
   1.257              (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
   1.258              (set-gravity world (Vector3f. 0 0 0))
   1.259              (light-up-everything world))
   1.260 -          (fn [_ _] (prop-view (list (prop))))])))))
   1.261 +          (fn [_ _] (prop-view (list (prop))))]))))))
   1.262  
   1.263  #+end_src
   1.264  
   1.265 @@ -908,7 +924,51 @@
   1.266         (light-up-everything world)
   1.267         (.setTimer world (NanoTimer.)))
   1.268       (fn [_ _]
   1.269 -       (dorun (motor-map [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0]))))))
   1.270 +       (dorun (motor-map [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0
   1.271 +     0]))))))
   1.272 +
   1.273 +(defn joint-proprioception [#^Node parts #^Node joint]
   1.274 +  (let [[obj-a obj-b] (joint-targets parts joint)
   1.275 +        joint-rot (.getWorldRotation joint)
   1.276 +        pre-inv-a (.inverse (.getWorldRotation obj-a))
   1.277 +        x (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_X))
   1.278 +        y (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Y))
   1.279 +        z (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Z))
   1.280 +
   1.281 +        x Vector3f/UNIT_Y
   1.282 +        y Vector3f/UNIT_Z
   1.283 +        z Vector3f/UNIT_X
   1.284 +
   1.285 +
   1.286 +        tmp-rot-a (.getWorldRotation obj-a)]
   1.287 +    (println-repl "x:" (.mult tmp-rot-a x))
   1.288 +    (println-repl "y:" (.mult tmp-rot-a y))
   1.289 +    (println-repl "z:" (.mult tmp-rot-a z))
   1.290 +    (println-repl "rot-a" (.getWorldRotation obj-a))
   1.291 +    (println-repl "rot-b" (.getWorldRotation obj-b))
   1.292 +    (println-repl "joint-rot" joint-rot)
   1.293 +    ;; this function will report proprioceptive information for the
   1.294 +    ;; joint.
   1.295 +    (fn []
   1.296 +      ;; x is the "twist" axis, y and z are the "bend" axes
   1.297 +      (let [rot-a (.getWorldRotation obj-a)
   1.298 +            ;;inv-a (.inverse rot-a)
   1.299 +            rot-b (.getWorldRotation obj-b)
   1.300 +            ;;relative (.mult rot-b inv-a)
   1.301 +            basis (doto (Matrix3f.)
   1.302 +                    (.setColumn 0 (.mult rot-a x))
   1.303 +                    (.setColumn 1 (.mult rot-a y))
   1.304 +                    (.setColumn 2 (.mult rot-a z)))
   1.305 +            rotation-about-joint
   1.306 +            (doto (Quaternion.)
   1.307 +              (.fromRotationMatrix 
   1.308 +               (.mult (.invert basis)
   1.309 +                      (.toRotationMatrix rot-b))))
   1.310 +            [yaw roll pitch]
   1.311 +            (seq (.toAngles rotation-about-joint nil))]
   1.312 +        ;;return euler angles of the quaternion around the new basis            
   1.313 +        [yaw roll pitch]))))
   1.314 +
   1.315  #+end_src
   1.316  
   1.317