1.1 Binary file assets/Models/creature1/try-again.blend has changed

     2.1 --- a/org/body.org	Thu Feb 02 03:17:11 2012 -0700
     2.2 +++ b/org/body.org	Thu Feb 02 12:49:11 2012 -0700
     2.3 @@ -63,42 +63,50 @@
     2.4      (seq (.getChildren joint-node))
     2.5      (do (println-repl "could not find JOINTS node") [])))
     2.6  
     2.7 +(defn right-handed? [vec1 vec2 vec3]
     2.8 +  (< 0 (.dot (.cross vec1 vec2) vec3)))
     2.9 +
    2.10 +(defn absolute-angle [vec1 vec2 axis]
    2.11 +  (let [angle (.angleBetween vec1 vec2)]
    2.12 +    (if (right-handed? vec1 vec2 axis)
    2.13 +      angle (- (* 2 Math/PI) angle))))
    2.14 +
    2.15 +
    2.16  (defn joint-proprioception [#^Node parts #^Node joint]
    2.17    (let [[obj-a obj-b] (joint-targets parts joint)
    2.18          joint-rot (.getWorldRotation joint)
    2.19 -        pre-inv-a (.inverse (.getWorldRotation obj-a))
    2.20 -        x (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_X))
    2.21 -        y (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Y))
    2.22 -        z (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Z))
    2.23 -        tmp-rot-a (.getWorldRotation obj-a)]
    2.24 -    (println-repl "x:" (.mult tmp-rot-a x))
    2.25 -    (println-repl "y:" (.mult tmp-rot-a y))
    2.26 -    (println-repl "z:" (.mult tmp-rot-a z))
    2.27 -    (println-repl "rot-a" (.getWorldRotation obj-a))
    2.28 -    (println-repl "rot-b" (.getWorldRotation obj-b))
    2.29 -    ;; this function will report proprioceptive information for the
    2.30 -    ;; joint.
    2.31 +        x0 (.mult joint-rot Vector3f/UNIT_X)
    2.32 +        y0 (.mult joint-rot Vector3f/UNIT_Y)
    2.33 +        z0 (.mult joint-rot Vector3f/UNIT_Z)]
    2.34 +    (println-repl "x:" x0)
    2.35 +    (println-repl "y:" y0)
    2.36 +    (println-repl "z:" z0)
    2.37 +    (println-repl "init-a:" (.getWorldRotation obj-a))
    2.38 +    (println-repl "init-b:" (.getWorldRotation obj-b))
    2.39 +
    2.40      (fn []
    2.41 -      ;; x is the "twist" axis, y and z are the "bend" axes
    2.42 -      (let [rot-a (.getWorldRotation obj-a)
    2.43 -            ;;inv-a (.inverse rot-a)
    2.44 -            rot-b (.getWorldRotation obj-b)
    2.45 -            ;;relative (.mult rot-b inv-a)
    2.46 -            basis (doto (Matrix3f.)
    2.47 -                    (.setColumn 0 (.mult rot-a x))
    2.48 -                    (.setColumn 1 (.mult rot-a y))
    2.49 -                    (.setColumn 2 (.mult rot-a z)))
    2.50 -            rotation-about-joint
    2.51 +      (let [rot-a (.clone (.getWorldRotation obj-a))
    2.52 +            rot-b (.clone (.getWorldRotation obj-b))
    2.53 +            x (.mult rot-a x0)
    2.54 +            y (.mult rot-a y0)
    2.55 +            z (.mult rot-a z0)
    2.56 +
    2.57 +            X (.mult rot-b x0)
    2.58 +            Y (.mult rot-b y0)
    2.59 +            Z (.mult rot-b z0)
    2.60 +            heading  (Math/atan2 (.dot X z) (.dot X x))
    2.61 +            pitch  (Math/atan2 (.dot X y) (.dot X x))
    2.62 +
    2.63 +            ;; rotate x-vector back to origin
    2.64 +            reverse
    2.65              (doto (Quaternion.)
    2.66 -              (.fromRotationMatrix 
    2.67 -               (.mult (.invert basis)
    2.68 -                      (.toRotationMatrix rot-b))))
    2.69 -            [yaw roll pitch]
    2.70 -            (seq (.toAngles rotation-about-joint nil))]
    2.71 -        ;;return euler angles of the quaternion around the new basis            
    2.72 -        [yaw roll pitch]
    2.73 -        ))))
    2.74 -
    2.75 +              (.fromAngleAxis
    2.76 +               (.angleBetween X x)
    2.77 +               (let [cross (.normalize (.cross X x))]
    2.78 +                 (if (= 0 (.length cross)) y cross))))
    2.79 +            roll (absolute-angle (.mult reverse Y) y x)]
    2.80 +        
    2.81 +        [heading pitch roll]))))
    2.82  
    2.83  (defn proprioception
    2.84    "Create a function that provides proprioceptive information about an
    2.85 @@ -161,17 +169,58 @@
    2.86          world-loop* (fn [world tpf]
    2.87                         (world-loop world tpf)
    2.88                         (splice-loop))]
    2.89 +    [root-node
    2.90 +     keymap*
    2.91 +     intilization
    2.92 +     world-loop*]))
    2.93  
    2.94 -     [root-node
    2.95 -      keymap*
    2.96 -      intilization
    2.97 -      world-loop*]))
    2.98 +(import java.awt.image.BufferedImage)
    2.99  
   2.100 -  
   2.101 +(defn draw-sprite [image sprite x y color ]
   2.102 +  (dorun
   2.103 +   (for [[u v] sprite]
   2.104 +     (.setRGB image (+ u x) (+ v y) color))))
   2.105 +
   2.106 +(defn view-angle
   2.107 +  "create a debug view of an angle"
   2.108 +  [color]
   2.109 +  (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
   2.110 +        previous (atom [25 25])
   2.111 +        sprite [[0 0] [0 1]
   2.112 +                [0 -1] [-1 0] [1 0]]] 
   2.113 +    (fn [angle]
   2.114 +      (let [angle (float angle)]
   2.115 +        (let [position
   2.116 +              [(+ 25 (int (* 20 (Math/cos angle))))
   2.117 +               (+ 25 (int (* -20 (Math/sin angle))))]]
   2.118 +          (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)
   2.119 +          (draw-sprite image sprite (position 0) (position 1) color)
   2.120 +          (reset! previous position))
   2.121 +        image))))
   2.122 +
   2.123 +(defn proprioception-debug-window
   2.124 +  []
   2.125 +  (let [heading (view-angle 0xFF0000)
   2.126 +        pitch (view-angle 0x00FF00)
   2.127 +        roll (view-angle 0xFFFFFF)
   2.128 +        v-heading (view-image)
   2.129 +        v-pitch (view-image)
   2.130 +        v-roll (view-image)
   2.131 +        ]
   2.132 +    (fn [prop-data]
   2.133 +      (dorun
   2.134 +       (map
   2.135 +        (fn [[h p r]]
   2.136 +          (v-heading (heading h))
   2.137 +          (v-pitch (pitch p))
   2.138 +          (v-roll (roll r)))
   2.139 +        prop-data)))))
   2.140 +
   2.141 +
   2.142  #+end_src
   2.143  
   2.144  #+results: proprioception
   2.145 -: #'cortex.body/proprioception
   2.146 +: #'cortex.body/proprioception-debug-window
   2.147  
   2.148  * Motor Control
   2.149  #+name: motor-control
   2.150 @@ -210,7 +259,8 @@
   2.151     com.jme3.math.ColorRGBA
   2.152     com.jme3.bullet.joints.Point2PointJoint
   2.153     com.jme3.bullet.control.RigidBodyControl
   2.154 -   com.jme3.system.NanoTimer))
   2.155 +   com.jme3.system.NanoTimer
   2.156 +   com.jme3.math.Quaternion))
   2.157  
   2.158  (defn worm-segments
   2.159    "Create multiple evenly spaced box segments. They're fabulous!"
   2.160 @@ -333,47 +383,6 @@
   2.161         (set-gravity world Vector3f/ZERO)
   2.162         )
   2.163       no-op)))
   2.164 -(import java.awt.image.BufferedImage)
   2.165 -
   2.166 -(defn draw-sprite [image sprite x y color ]
   2.167 -  (dorun
   2.168 -   (for [[u v] sprite]
   2.169 -     (.setRGB image (+ u x) (+ v y) color))))
   2.170 -
   2.171 -(defn view-angle
   2.172 -  "create a debug view of an angle"
   2.173 -  [color]
   2.174 -  (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
   2.175 -        previous (atom [25 25])
   2.176 -        sprite [[0 0] [0 1]
   2.177 -                [0 -1] [-1 0] [1 0]]] 
   2.178 -    (fn [angle]
   2.179 -      (let [angle (float angle)]
   2.180 -        (let [position
   2.181 -              [(+ 25 (int (* 20 (Math/cos angle))))
   2.182 -               (+ 25 (int (* 20(Math/sin angle))))]]
   2.183 -          (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)
   2.184 -          (draw-sprite image sprite (position 0) (position 1) color)
   2.185 -          (reset! previous position))
   2.186 -        image))))
   2.187 -
   2.188 -(defn proprioception-debug-window
   2.189 -  []
   2.190 -  (let [yaw (view-angle 0xFF0000)
   2.191 -        roll (view-angle 0x00FF00)
   2.192 -        pitch (view-angle 0xFFFFFF)
   2.193 -        v-yaw (view-image)
   2.194 -        v-roll (view-image)
   2.195 -        v-pitch (view-image)
   2.196 -        ]
   2.197 -    (fn [prop-data]
   2.198 -      (dorun
   2.199 -       (map
   2.200 -        (fn [[y r p]]
   2.201 -          (v-yaw (yaw y))
   2.202 -          (v-roll (roll r))
   2.203 -          (v-pitch (pitch p)))
   2.204 -        prop-data)))))
   2.205  (comment  
   2.206       
   2.207  (defn proprioception-debug-window
   2.208 @@ -406,25 +415,32 @@
   2.209     and changes yaw. key-v/key-b will spin the blue segment clockwise
   2.210     and counterclockwise, and only affect roll."
   2.211    []
   2.212 -  (let [hand    (box 1 0.2 0.2 :position (Vector3f. 0 2 0)
   2.213 +  (let [hand    (box 0.2 1 0.2 :position (Vector3f. 0 0 0)
   2.214                       :mass 0 :color ColorRGBA/Green :name "hand")
   2.215 -        finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0)
   2.216 +        finger (box 0.2 1 0.2 :position (Vector3f. 0 2.4 0)
   2.217                      :mass 1 :color ColorRGBA/Red :name "finger")
   2.218          joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow
   2.219 -                        :position (Vector3f. 1.2 2 0)
   2.220 +                        :position (Vector3f. 0 1.2 0)
   2.221 +                        :rotation (doto (Quaternion.)
   2.222 +                                    (.fromAngleAxis
   2.223 +                                     (/ Math/PI 2)
   2.224 +                                     (Vector3f. 0 0 1)))
   2.225                          :physical? false)
   2.226 -        joint (join-at-point hand finger (Vector3f. 1.2 2 0 ))
   2.227 +        joint (join-at-point hand finger (Vector3f. 0 1.2 0 ))
   2.228          creature (nodify [hand finger joint-node])
   2.229 +        finger-control (.getControl finger RigidBodyControl)
   2.230 +        hand-control (.getControl hand RigidBodyControl)]
   2.231 +    
   2.232 +
   2.233 +    (let
   2.234          ;; *******************************************
   2.235                  
   2.236 -        floor   (box 10 10 10 :position (Vector3f. 0 -15 0)
   2.237 +        [floor   (box 10 10 10 :position (Vector3f. 0 -15 0)
   2.238                       :mass 0 :color ColorRGBA/Gray)
   2.239          
   2.240          root (nodify [creature floor])
   2.241          prop (joint-proprioception creature joint-node)
   2.242          prop-view (proprioception-debug-window)
   2.243 -        finger-control (.getControl finger RigidBodyControl)
   2.244 -        hand-control (.getControl hand RigidBodyControl)
   2.245          
   2.246          controls
   2.247          (merge standard-debug-controls
   2.248 @@ -471,14 +487,14 @@
   2.249         (with-movement
   2.250           finger
   2.251           ["key-r" "key-t" "key-f" "key-g" "key-v" "key-b"]
   2.252 -         [10 10 10 10 1 1]
   2.253 +         [1 1 10 10 10 10]
   2.254           [root
   2.255            controls
   2.256            (fn [world]
   2.257              (.setTimer world (com.aurellem.capture.RatchetTimer. 60))
   2.258              (set-gravity world (Vector3f. 0 0 0))
   2.259              (light-up-everything world))
   2.260 -          (fn [_ _] (prop-view (list (prop))))])))))
   2.261 +          (fn [_ _] (prop-view (list (prop))))]))))))
   2.262  
   2.263  #+end_src
   2.264  
   2.265 @@ -908,7 +924,51 @@
   2.266         (light-up-everything world)
   2.267         (.setTimer world (NanoTimer.)))
   2.268       (fn [_ _]
   2.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]))))))
   2.270 +       (dorun (motor-map [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0
   2.271 +     0]))))))
   2.272 +
   2.273 +(defn joint-proprioception [#^Node parts #^Node joint]
   2.274 +  (let [[obj-a obj-b] (joint-targets parts joint)
   2.275 +        joint-rot (.getWorldRotation joint)
   2.276 +        pre-inv-a (.inverse (.getWorldRotation obj-a))
   2.277 +        x (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_X))
   2.278 +        y (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Y))
   2.279 +        z (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Z))
   2.280 +
   2.281 +        x Vector3f/UNIT_Y
   2.282 +        y Vector3f/UNIT_Z
   2.283 +        z Vector3f/UNIT_X
   2.284 +
   2.285 +
   2.286 +        tmp-rot-a (.getWorldRotation obj-a)]
   2.287 +    (println-repl "x:" (.mult tmp-rot-a x))
   2.288 +    (println-repl "y:" (.mult tmp-rot-a y))
   2.289 +    (println-repl "z:" (.mult tmp-rot-a z))
   2.290 +    (println-repl "rot-a" (.getWorldRotation obj-a))
   2.291 +    (println-repl "rot-b" (.getWorldRotation obj-b))
   2.292 +    (println-repl "joint-rot" joint-rot)
   2.293 +    ;; this function will report proprioceptive information for the
   2.294 +    ;; joint.
   2.295 +    (fn []
   2.296 +      ;; x is the "twist" axis, y and z are the "bend" axes
   2.297 +      (let [rot-a (.getWorldRotation obj-a)
   2.298 +            ;;inv-a (.inverse rot-a)
   2.299 +            rot-b (.getWorldRotation obj-b)
   2.300 +            ;;relative (.mult rot-b inv-a)
   2.301 +            basis (doto (Matrix3f.)
   2.302 +                    (.setColumn 0 (.mult rot-a x))
   2.303 +                    (.setColumn 1 (.mult rot-a y))
   2.304 +                    (.setColumn 2 (.mult rot-a z)))
   2.305 +            rotation-about-joint
   2.306 +            (doto (Quaternion.)
   2.307 +              (.fromRotationMatrix 
   2.308 +               (.mult (.invert basis)
   2.309 +                      (.toRotationMatrix rot-b))))
   2.310 +            [yaw roll pitch]
   2.311 +            (seq (.toAngles rotation-about-joint nil))]
   2.312 +        ;;return euler angles of the quaternion around the new basis            
   2.313 +        [yaw roll pitch]))))
   2.314 +
   2.315  #+end_src
   2.316  
   2.317  

     3.1 --- a/org/test-creature.org	Thu Feb 02 03:17:11 2012 -0700
     3.2 +++ b/org/test-creature.org	Thu Feb 02 12:49:11 2012 -0700
     3.3 @@ -1086,53 +1086,6 @@
     3.4         0xFF
     3.5         (.getRGB image x 0)))))
     3.6  
     3.7 -
     3.8 -(defn draw-sprite [image sprite x y color ]
     3.9 -  (dorun
    3.10 -   (for [[u v] sprite]
    3.11 -     (.setRGB image (+ u x) (+ v y) color))))
    3.12 -
    3.13 -(defn view-angle
    3.14 -  "create a debug view of an angle"
    3.15 -  [color]
    3.16 -  (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
    3.17 -        previous (atom [25 25])
    3.18 -        sprite [[0 0] [0 1]
    3.19 -                [0 -1] [-1 0] [1 0]]] 
    3.20 -    (fn [angle]
    3.21 -      (let [angle (float angle)]
    3.22 -        (let [position
    3.23 -              [(+ 25 (int (* 20 (Math/cos angle))))
    3.24 -               (+ 25 (int (* 20(Math/sin angle))))]]
    3.25 -          (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)
    3.26 -          (draw-sprite image sprite (position 0) (position 1) color)
    3.27 -          (reset! previous position))
    3.28 -        image))))
    3.29 -
    3.30 -(defn proprioception-debug-window
    3.31 -  []
    3.32 -  (let [yaw (view-angle 0xFF0000)
    3.33 -        roll (view-angle 0x00FF00)
    3.34 -        pitch (view-angle 0xFFFFFF)
    3.35 -        v-yaw (view-image)
    3.36 -        v-roll (view-image)
    3.37 -        v-pitch (view-image)
    3.38 -        ]
    3.39 -    (fn [prop-data]
    3.40 -      (dorun
    3.41 -       (map
    3.42 -        (fn [[y r p]]
    3.43 -          (v-yaw (yaw y))
    3.44 -          (v-roll (roll r))
    3.45 -          (v-pitch (pitch p)))
    3.46 -        prop-data)))))
    3.47 -
    3.48 -
    3.49 -
    3.50 -
    3.51 -
    3.52 -
    3.53 -
    3.54  (defn test-creature [thing]
    3.55    (let [x-axis
    3.56          (box 1 0.01 0.01 :physical? false :color ColorRGBA/Red)
    3.57 @@ -1190,7 +1143,7 @@
    3.58                     (comp (view-image) BufferedImage!))
    3.59            
    3.60            (add-eye world (.getCamera world) no-op)
    3.61 -          (set-gravity world (Vector3f. 0 0 0))
    3.62 +          ;;(set-gravity world (Vector3f. 0 0 0))
    3.63            ;;(com.aurellem.capture.Capture/captureVideo
    3.64            ;; world (file-str "/home/r/proj/ai-videos/hand"))
    3.65            ;;(.setTimer world (RatchetTimer. 60))