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

     2.1 --- a/org/body.org	Wed Feb 01 02:27:18 2012 -0700
     2.2 +++ b/org/body.org	Wed Feb 01 02:44:07 2012 -0700
     2.3 @@ -17,144 +17,6 @@
     2.4     com.jme3.math.Matrix3f
     2.5     com.jme3.bullet.control.RigidBodyControl))
     2.6  
     2.7 -(comment
     2.8 -  (defn joint-proprioception
     2.9 -  "Relative position information for a two-part system connected by a
    2.10 -   joint. Gives the pitch, yaw, and roll of the 'B' object relative to
    2.11 -   the 'A' object, as determined by the joint."
    2.12 -  [joint]
    2.13 -  (let [object-a (.getUserObject (.getBodyA joint))
    2.14 -        object-b (.getUserObject (.getBodyB joint))
    2.15 -        arm-a
    2.16 -        (.normalize
    2.17 -         (.subtract 
    2.18 -          (.localToWorld object-a (.getPivotA joint) nil)
    2.19 -          (.getWorldTranslation object-a)))
    2.20 -
    2.21 -        ;; this is probably wrong!
    2.22 -        rotate-a
    2.23 -         (doto (Matrix3f.)
    2.24 -           (.fromStartEndVectors arm-a Vector3f/UNIT_X))
    2.25 -        
    2.26 -        arm-b
    2.27 -        (.mult
    2.28 -         rotate-a 
    2.29 -         (.normalize
    2.30 -          (.subtract 
    2.31 -           (.localToWorld object-b (.getPivotB joint) nil)
    2.32 -           (.getWorldTranslation object-b))))
    2.33 -        pitch
    2.34 -        (.angleBetween
    2.35 -         (.normalize (Vector2f. (.getX arm-b) (.getY arm-b)))
    2.36 -         (Vector2f. 1 0))
    2.37 -        yaw
    2.38 -        (.angleBetween
    2.39 -         (.normalize (Vector2f. (.getX arm-b) (.getZ arm-b)))
    2.40 -         (Vector2f. 1 0))
    2.41 -                     
    2.42 -        roll
    2.43 -        (project-quaternion
    2.44 -         (.mult 
    2.45 -          (.getLocalRotation object-b)
    2.46 -          (doto (Quaternion.)
    2.47 -            (.fromRotationMatrix rotate-a)))
    2.48 -         arm-b)]
    2.49 -    ;;(println-repl (.getName object-a) (.getName object-b))
    2.50 -    [pitch yaw roll]))
    2.51 -)
    2.52 -
    2.53 -(defn any-orthogonal
    2.54 -  "Generate an arbitray (but stable) orthogonal vector to a given
    2.55 -   vector."
    2.56 -  [vector]
    2.57 -  (let [x (.getX vector)
    2.58 -        y (.getY vector)
    2.59 -        z (.getZ vector)]
    2.60 -    (cond
    2.61 -     (not= x (float 0)) (Vector3f. (- z) 0 x)
    2.62 -     (not= y (float 0)) (Vector3f. 0 (- z) y)
    2.63 -     (not= z (float 0)) (Vector3f. 0 (- z) y)
    2.64 -     true Vector3f/ZERO)))
    2.65 -
    2.66 -(comment
    2.67 -(defn project-quaternion
    2.68 -  "From http://stackoverflow.com/questions/3684269/
    2.69 -   component-of-a-quaternion-rotation-around-an-axis.
    2.70 -
    2.71 -   Determine the amount of rotation a quaternion will
    2.72 -   cause about a given axis."
    2.73 -  [#^Quaternion q #^Vector3f axis]
    2.74 -  (let [basis-1 (any-orthogonal axis)
    2.75 -        basis-2 (.cross axis basis-1)
    2.76 -        rotated (.mult q basis-1)
    2.77 -        alpha (.dot basis-1 (.project rotated basis-1))
    2.78 -        beta  (.dot basis-2 (.project rotated basis-2))]
    2.79 -    (Math/atan2 beta alpha)))
    2.80 -)
    2.81 -
    2.82 -(defn right-handed? [vec1 vec2 vec3]
    2.83 -  (< 0 (.dot (.cross vec1 vec2) vec3)))
    2.84 -
    2.85 -(defn absolute-angle [vec1 vec2 axis]
    2.86 -  (let [angle (.angleBetween vec1 vec2)]
    2.87 -    (if (right-handed? vec1 vec2 axis)
    2.88 -      angle (- (* 2 Math/PI) angle))))
    2.89 -
    2.90 -(defn angle-min [& angles]
    2.91 -  (first
    2.92 -   (sort-by 
    2.93 -    (fn [angle]
    2.94 -      (let [in-circle (Math/abs (rem angle (* 2 Math/PI)))]
    2.95 -        (min in-circle
    2.96 -             (- (* Math/PI 2) in-circle))))
    2.97 -    angles)))
    2.98 -
    2.99 -(defn project-quaternion
   2.100 -  "From http://stackoverflow.com/questions/3684269/
   2.101 -   component-of-a-quaternion-rotation-around-an-axis.
   2.102 -
   2.103 -   Determine the amount of rotation a quaternion will
   2.104 -   cause about a given axis."
   2.105 -  [#^Quaternion q #^Vector3f axis]
   2.106 -  (let [axis (.normalize axis)
   2.107 -        basis-1 (.normalize (any-orthogonal axis))
   2.108 -        basis-2 (.cross axis basis-1)
   2.109 -        rotated-1 (.mult q basis-1)
   2.110 -        basis-1* (.normalize
   2.111 -                          (.add (.project rotated-1 basis-1)
   2.112 -                                (.project rotated-1 basis-2)))
   2.113 -        rotated-2 (.mult q basis-2)
   2.114 -        basis-2* (.normalize
   2.115 -                          (.add (.project rotated-2 basis-1)
   2.116 -                                (.project rotated-2 basis-2)))
   2.117 -        angle-1
   2.118 -        (absolute-angle basis-1 basis-1* axis)
   2.119 -        angle-2
   2.120 -        (absolute-angle basis-2 basis-2* axis)
   2.121 -
   2.122 -
   2.123 -        angle (angle-min angle-1 angle-2)
   2.124 -        ]
   2.125 -  
   2.126 -
   2.127 -    ;; be sure to get sign from cross product
   2.128 -    (if false
   2.129 -      (do
   2.130 -        (println-repl "axis" axis)
   2.131 -        (println-repl "basis-1" basis-1)
   2.132 -        (println-repl "basis-2" basis-2)
   2.133 -        (println-repl "rotated-1" rotated-1)
   2.134 -        (println-repl "rotated-2" rotated-2)
   2.135 -        (println-repl "basis-1*" basis-1*)
   2.136 -        (println-repl "basis-2*" basis-2*)
   2.137 -        (println-repl "angle-1" angle-1)
   2.138 -        (println-repl "angle-2" angle-2)
   2.139 -        
   2.140 -        (println-repl "angle" angle)
   2.141 -        (println-repl "")))
   2.142 -    angle))
   2.143 -    
   2.144 -
   2.145  (import com.jme3.scene.Node)
   2.146  
   2.147  (defn joint-proprioception [#^Node parts #^Node joint]
   2.148 @@ -187,53 +49,15 @@
   2.149          ))))
   2.150  
   2.151  
   2.152 -(comment
   2.153 -
   2.154 -(defn joint-proprioception
   2.155 -  [joint]
   2.156 -  (let [object-a (.getUserObject (.getBodyA joint))
   2.157 -        object-b (.getUserObject (.getBodyB joint))
   2.158 -        rot-a (.clone (.getWorldRotation object-a))
   2.159 -        rot-b (.clone (.getWorldRotation object-b))
   2.160 -        ]
   2.161 -    
   2.162 -    (.mult rot-b (.inverse rot-a))
   2.163 -    
   2.164 -    ;; object-a == hand
   2.165 -    ;; object-b == finger
   2.166 -    ))
   2.167 -)
   2.168 -;; (defn joint-proprioception*
   2.169 -;;   [joint]
   2.170 -;;   (let [object-a (.getUserObject (.getBodyA joint))
   2.171 -;;         object-b (.getUserObject (.getBodyB joint))
   2.172 -
   2.173 -;;         rotate-a (.clone (.getWorldRotation object-a))
   2.174 -;;         rotate-b (.clone (.getWorldRotation object-b))
   2.175 -
   2.176 -;;         rotate-rel (.mult rotate-b (.inverse rotate-a))
   2.177 -;;         ]
   2.178 -;;     ((comp vec map) (partial project-quaternion rotate-rel)
   2.179 -;;          [Vector3f/UNIT_X
   2.180 -;;           Vector3f/UNIT_Y
   2.181 -;;           Vector3f/UNIT_Z])))
   2.182 -
   2.183 -
   2.184  (defn proprioception
   2.185    "Create a function that provides proprioceptive information about an
   2.186    entire body."
   2.187 -  [body]
   2.188 +  [#^Node creature]
   2.189    ;; extract the body's joints
   2.190 -  (let [joints 
   2.191 -        (distinct
   2.192 -         (reduce
   2.193 -          concat
   2.194 -          (map #(.getJoints %)
   2.195 -               (keep
   2.196 -                #(.getControl % RigidBodyControl)
   2.197 -                (node-seq body)))))]
   2.198 +  (let [joints (cortex.silly/creature-joints creature)
   2.199 +        senses (map (partial joint-proprioception creature) joints)]
   2.200      (fn []
   2.201 -      (map joint-proprioception joints))))
   2.202 +      (map #(%) senses))))
   2.203    
   2.204  #+end_src
   2.205  

     3.1 --- a/org/test-creature.org	Wed Feb 01 02:27:18 2012 -0700
     3.2 +++ b/org/test-creature.org	Wed Feb 01 02:44:07 2012 -0700
     3.3 @@ -1058,16 +1058,20 @@
     3.4  ;; these are the functions that provide world i/o, chinese-room style
     3.5  
     3.6  
     3.7 +(defn creature-joints 
     3.8 +  "Return the children of the creature's \"joints\" node."
     3.9 +  [#^Node creature]
    3.10 +  (if-let [joint-node (.getChild creature "joints")]
    3.11 +    (seq (.getChildren joint-node))
    3.12 +    (do (println-repl "could not find JOINTS node") [])))
    3.13 +
    3.14  
    3.15  (defn blender-creature
    3.16    "Return a creature with all joints in place."
    3.17    [blender-path]
    3.18    (let [model (load-blender-model blender-path)
    3.19 -        joints
    3.20 -        (if-let [joint-node (.getChild model "joints")]
    3.21 -          (seq (.getChildren joint-node))
    3.22 -          (do (println-repl "could not find joints node") []))]
    3.23 -  (assemble-creature model joints)))
    3.24 +        joints (creature-joints model)]
    3.25 +    (assemble-creature model joints)))
    3.26  
    3.27  (defn gray-scale [num]
    3.28    (+ num
    3.29 @@ -1167,7 +1171,49 @@
    3.30            [(rad->deg yaw)
    3.31             (rad->deg pitch)
    3.32             (rad->deg roll)])
    3.33 -       sensor-data)))))
    3.34 +        sensor-data)))))
    3.35 +
    3.36 +
    3.37 +(defn draw-sprite [image sprite x y color ]
    3.38 +  (dorun
    3.39 +   (for [[u v] sprite]
    3.40 +     (.setRGB image (+ u x) (+ v y) color))))
    3.41 +
    3.42 +(defn view-angle
    3.43 +  "create a debug view of an angle"
    3.44 +  [color]
    3.45 +  (let [image (BufferedImage. 50 50 BufferedImage/TYPE_INT_RGB)
    3.46 +        previous (atom [25 25])
    3.47 +        sprite [[0 0] [0 1]
    3.48 +                [0 -1] [-1 0] [1 0]]] 
    3.49 +    (fn [angle]
    3.50 +      (let [angle (float angle)]
    3.51 +        (let [position
    3.52 +              [(+ 25 (int (* 20 (Math/cos angle))))
    3.53 +               (+ 25 (int (* 20(Math/sin angle))))]]
    3.54 +          (draw-sprite image sprite (@previous 0) (@previous 1) 0x000000)
    3.55 +          (draw-sprite image sprite (position 0) (position 1) color)
    3.56 +          (reset! previous position))
    3.57 +        image))))
    3.58 +
    3.59 +(defn proprioception-debug-window
    3.60 +  []
    3.61 +  (let [yaw (view-angle 0xFF0000)
    3.62 +        roll (view-angle 0x00FF00)
    3.63 +        pitch (view-angle 0xFFFFFF)
    3.64 +        v-yaw (view-image)
    3.65 +        v-roll (view-image)
    3.66 +        v-pitch (view-image)
    3.67 +        ]
    3.68 +    (fn [prop-data]
    3.69 +      (dorun
    3.70 +       (map
    3.71 +        (fn [[y r p]]
    3.72 +          (v-yaw (yaw y))
    3.73 +          (v-roll (roll r))
    3.74 +          (v-pitch (pitch p)))
    3.75 +        prop-data)))))
    3.76 +
    3.77  
    3.78  
    3.79