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

     2.1 --- a/org/body.org	Tue Jan 31 01:40:47 2012 -0700
     2.2 +++ b/org/body.org	Wed Feb 01 02:27:18 2012 -0700
     2.3 @@ -17,67 +17,8 @@
     2.4     com.jme3.math.Matrix3f
     2.5     com.jme3.bullet.control.RigidBodyControl))
     2.6  
     2.7 -
     2.8 -
     2.9 -(defn quaternion-decompose [#^Quaternion q]
    2.10 -  (map
    2.11 -   #(arc-between % (.rotate q %))
    2.12 -   [Vector3f/UNIT_X
    2.13 -    Vector3f/UNIT_Y
    2.14 -    Vector3f/UNIT_Z]))
    2.15 -
    2.16 -(defn any-orthogonal
    2.17 -  "Generate an arbitray (but stable) orthogonal vector to a given
    2.18 -   vector."
    2.19 -  [vector]
    2.20 -  (let [x (.getX vector)
    2.21 -        y (.getY vector)
    2.22 -        z (.getZ vector)]
    2.23 -    (cond
    2.24 -     (not= x (float 0)) (Vector3f. (- z) 0 x)
    2.25 -     (not= y (float 0)) (Vector3f. 0 (- z) y)
    2.26 -     (not= z (float 0)) (Vector3f. 0 (- z) y)
    2.27 -     true Vector3f/ZERO)))
    2.28 -
    2.29 -(defn project-quaternion
    2.30 -  "From http://stackoverflow.com/questions/3684269/
    2.31 -   component-of-a-quaternion-rotation-around-an-axis.
    2.32 -
    2.33 -   Determine the amount of rotation a quaternion will
    2.34 -   cause about a given axis."
    2.35 -  [#^Quaternion q #^Vector3f axis]
    2.36 -  (let [basis-1 (any-orthogonal axis)
    2.37 -        basis-2 (.cross axis basis-1)
    2.38 -        rotated (.mult q basis-1)
    2.39 -        alpha (.dot basis-1 (.project rotated basis-1))
    2.40 -        beta  (.dot basis-2 (.project rotated basis-2))]
    2.41 -    (Math/atan2 beta alpha)))
    2.42 -
    2.43 -(defn right-handed? [vec1 vec2 vec3]
    2.44 -  (< 0 (.dot (.cross vec1 vec2) vec3)))
    2.45 -
    2.46 -(defn project-quaternion
    2.47 -  "From http://stackoverflow.com/questions/3684269/
    2.48 -   component-of-a-quaternion-rotation-around-an-axis.
    2.49 -
    2.50 -   Determine the amount of rotation a quaternion will
    2.51 -   cause about a given axis."
    2.52 -  [#^Quaternion q #^Vector3f axis]
    2.53 -  (let [axis (.normalize axis)
    2.54 -        basis-1 (any-orthogonal axis)
    2.55 -        basis-2 (.cross axis basis-1)
    2.56 -        rotated (.mult q basis-1)
    2.57 -        rotated-in-plane (.add (.project rotated basis-1)
    2.58 -                               (.project rotated basis-2))]
    2.59 -
    2.60 -    ;; be sure to get sign from cross product
    2.61 -    (if (right-handed? basis-1 rotated-in-plane axis)
    2.62 -      (.angleBetween rotated-in-plane basis-1)
    2.63 -      (- (* 2 Math/PI) (.angleBetween rotated-in-plane basis-1)))))
    2.64 -
    2.65 -
    2.66 -
    2.67 -(defn joint-proprioception
    2.68 +(comment
    2.69 +  (defn joint-proprioception
    2.70    "Relative position information for a two-part system connected by a
    2.71     joint. Gives the pitch, yaw, and roll of the 'B' object relative to
    2.72     the 'A' object, as determined by the joint."
    2.73 @@ -120,63 +61,148 @@
    2.74           arm-b)]
    2.75      ;;(println-repl (.getName object-a) (.getName object-b))
    2.76      [pitch yaw roll]))
    2.77 +)
    2.78  
    2.79 +(defn any-orthogonal
    2.80 +  "Generate an arbitray (but stable) orthogonal vector to a given
    2.81 +   vector."
    2.82 +  [vector]
    2.83 +  (let [x (.getX vector)
    2.84 +        y (.getY vector)
    2.85 +        z (.getZ vector)]
    2.86 +    (cond
    2.87 +     (not= x (float 0)) (Vector3f. (- z) 0 x)
    2.88 +     (not= y (float 0)) (Vector3f. 0 (- z) y)
    2.89 +     (not= z (float 0)) (Vector3f. 0 (- z) y)
    2.90 +     true Vector3f/ZERO)))
    2.91  
    2.92 +(comment
    2.93 +(defn project-quaternion
    2.94 +  "From http://stackoverflow.com/questions/3684269/
    2.95 +   component-of-a-quaternion-rotation-around-an-axis.
    2.96  
    2.97 +   Determine the amount of rotation a quaternion will
    2.98 +   cause about a given axis."
    2.99 +  [#^Quaternion q #^Vector3f axis]
   2.100 +  (let [basis-1 (any-orthogonal axis)
   2.101 +        basis-2 (.cross axis basis-1)
   2.102 +        rotated (.mult q basis-1)
   2.103 +        alpha (.dot basis-1 (.project rotated basis-1))
   2.104 +        beta  (.dot basis-2 (.project rotated basis-2))]
   2.105 +    (Math/atan2 beta alpha)))
   2.106 +)
   2.107  
   2.108 -(defn absolute-angle-between
   2.109 -  [vec-1 vec-2]
   2.110 +(defn right-handed? [vec1 vec2 vec3]
   2.111 +  (< 0 (.dot (.cross vec1 vec2) vec3)))
   2.112 +
   2.113 +(defn absolute-angle [vec1 vec2 axis]
   2.114 +  (let [angle (.angleBetween vec1 vec2)]
   2.115 +    (if (right-handed? vec1 vec2 axis)
   2.116 +      angle (- (* 2 Math/PI) angle))))
   2.117 +
   2.118 +(defn angle-min [& angles]
   2.119 +  (first
   2.120 +   (sort-by 
   2.121 +    (fn [angle]
   2.122 +      (let [in-circle (Math/abs (rem angle (* 2 Math/PI)))]
   2.123 +        (min in-circle
   2.124 +             (- (* Math/PI 2) in-circle))))
   2.125 +    angles)))
   2.126 +
   2.127 +(defn project-quaternion
   2.128 +  "From http://stackoverflow.com/questions/3684269/
   2.129 +   component-of-a-quaternion-rotation-around-an-axis.
   2.130 +
   2.131 +   Determine the amount of rotation a quaternion will
   2.132 +   cause about a given axis."
   2.133 +  [#^Quaternion q #^Vector3f axis]
   2.134 +  (let [axis (.normalize axis)
   2.135 +        basis-1 (.normalize (any-orthogonal axis))
   2.136 +        basis-2 (.cross axis basis-1)
   2.137 +        rotated-1 (.mult q basis-1)
   2.138 +        basis-1* (.normalize
   2.139 +                          (.add (.project rotated-1 basis-1)
   2.140 +                                (.project rotated-1 basis-2)))
   2.141 +        rotated-2 (.mult q basis-2)
   2.142 +        basis-2* (.normalize
   2.143 +                          (.add (.project rotated-2 basis-1)
   2.144 +                                (.project rotated-2 basis-2)))
   2.145 +        angle-1
   2.146 +        (absolute-angle basis-1 basis-1* axis)
   2.147 +        angle-2
   2.148 +        (absolute-angle basis-2 basis-2* axis)
   2.149 +
   2.150 +
   2.151 +        angle (angle-min angle-1 angle-2)
   2.152 +        ]
   2.153    
   2.154  
   2.155 +    ;; be sure to get sign from cross product
   2.156 +    (if false
   2.157 +      (do
   2.158 +        (println-repl "axis" axis)
   2.159 +        (println-repl "basis-1" basis-1)
   2.160 +        (println-repl "basis-2" basis-2)
   2.161 +        (println-repl "rotated-1" rotated-1)
   2.162 +        (println-repl "rotated-2" rotated-2)
   2.163 +        (println-repl "basis-1*" basis-1*)
   2.164 +        (println-repl "basis-2*" basis-2*)
   2.165 +        (println-repl "angle-1" angle-1)
   2.166 +        (println-repl "angle-2" angle-2)
   2.167 +        
   2.168 +        (println-repl "angle" angle)
   2.169 +        (println-repl "")))
   2.170 +    angle))
   2.171 +    
   2.172  
   2.173 +(import com.jme3.scene.Node)
   2.174 +
   2.175 +(defn joint-proprioception [#^Node parts #^Node joint]
   2.176 +  (let [[obj-a obj-b] (cortex.silly/joint-targets parts joint)
   2.177 +        joint-rot (.getWorldRotation joint)
   2.178 +        x (.mult joint-rot Vector3f/UNIT_X)
   2.179 +        y (.mult joint-rot Vector3f/UNIT_Y)
   2.180 +        z (.mult joint-rot Vector3f/UNIT_Z)]
   2.181 +    ;; this function will report proprioceptive information for the
   2.182 +    ;; joint 
   2.183 +    (fn []
   2.184 +      ;; x is the "twist" axis, y and z are the "bend" axes
   2.185 +      (let [rot-a (.getWorldRotation obj-a)
   2.186 +            rot-b (.getWorldRotation obj-b)
   2.187 +            relative (.mult (.inverse rot-a) rot-b)
   2.188 +            basis (doto (Matrix3f.)
   2.189 +                    (.setColumn 0 x)
   2.190 +                    (.setColumn 1 y)
   2.191 +                    (.setColumn 2 z))
   2.192 +            rotation-about-joint
   2.193 +            (doto (Quaternion.)
   2.194 +              (.fromRotationMatrix 
   2.195 +               (.mult (.invert basis)
   2.196 +                      (.toRotationMatrix relative))))
   2.197 +            [yaw roll pitch]
   2.198 +            (seq (.toAngles rotation-about-joint nil))]
   2.199 +        ;;return euler angles of the quaternion around the new basis            
   2.200 +        ;;[yaw pitch roll]
   2.201 +        [yaw roll pitch]
   2.202 +        ))))
   2.203 +
   2.204 +
   2.205 +(comment
   2.206  
   2.207  (defn joint-proprioception
   2.208    [joint]
   2.209    (let [object-a (.getUserObject (.getBodyA joint))
   2.210          object-b (.getUserObject (.getBodyB joint))
   2.211 -
   2.212 -        arm-a
   2.213 -        (.normalize
   2.214 -         (.subtract 
   2.215 -          (.localToWorld object-a (.getPivotA joint) nil)
   2.216 -          (.getWorldTranslation object-a)))
   2.217 -        arm-b
   2.218 -        (.normalize
   2.219 -         (.subtract 
   2.220 -          (.localToWorld object-b (.getPivotB joint) nil)
   2.221 -          (.getWorldTranslation object-b)))
   2.222 -
   2.223 -        rotate-a (.clone (.getWorldRotation object-a))
   2.224 -        unrotate-a (.inverse (.getWorldRotation object-a))
   2.225 -
   2.226 -        canonical-arm-a (.mult unrotate-a arm-a)
   2.227 -
   2.228 -        basis-1 (any-orthogonal canonical-arm-a)
   2.229 -        basis-2 (.normalize (.cross basis-1 canonical-arm-a))
   2.230 -
   2.231 -        pitch (.angleBetween arm-b basis-1)
   2.232 -        yaw (.angleBetween arm-b basis-2)
   2.233 -
   2.234 -        twist-a
   2.235 -        (project-quaternion
   2.236 -         (.getWorldRotation object-a) arm-a)
   2.237 -        
   2.238 -        twist-b
   2.239 -        (project-quaternion
   2.240 -         (.getWorldRotation object-b) arm-b)
   2.241 -
   2.242 -        roll (- twist-b 0)
   2.243 -        ;; "un-rotate" arm-a to it's canonical position, get two
   2.244 -        ;; orthogonal basis vectors. Rotate those two vectors back to
   2.245 -        ;; the correct position get the rotations between them.
   2.246 -
   2.247 -        ;; twist is the rotation about arm-a of obj-b minus the
   2.248 -        ;; rotation about arm-a of obj-a
   2.249 +        rot-a (.clone (.getWorldRotation object-a))
   2.250 +        rot-b (.clone (.getWorldRotation object-b))
   2.251          ]
   2.252 +    
   2.253 +    (.mult rot-b (.inverse rot-a))
   2.254 +    
   2.255      ;; object-a == hand
   2.256      ;; object-b == finger
   2.257 -    [pitch yaw roll]))
   2.258 -
   2.259 +    ))
   2.260 +)
   2.261  ;; (defn joint-proprioception*
   2.262  ;;   [joint]
   2.263  ;;   (let [object-a (.getUserObject (.getBodyA joint))
   2.264 @@ -211,6 +237,9 @@
   2.265    
   2.266  #+end_src
   2.267  
   2.268 +#+results: proprioception
   2.269 +: #'cortex.body/proprioception
   2.270 +
   2.271  * Motor Control
   2.272  #+name: motor-control
   2.273  #+begin_src clojure
   2.274 @@ -346,7 +375,7 @@
   2.275     nil
   2.276     ))
   2.277  
   2.278 -
   2.279 +(import com.jme3.bullet.collision.PhysicsCollisionObject)
   2.280  
   2.281  (defn blab-* []
   2.282    (let [hand   (box 0.5 0.2 0.2 :position (Vector3f. 0 0 0)
   2.283 @@ -397,21 +426,30 @@
   2.284  (defn proprioception-debug-window
   2.285    []
   2.286    (let [yaw (view-angle 0xFF0000)
   2.287 -        pitch (view-angle 0x00FF00)
   2.288 -        roll (view-angle 0xFFFFFF)
   2.289 +        roll (view-angle 0x00FF00)
   2.290 +        pitch (view-angle 0xFFFFFF)
   2.291          v-yaw (view-image)
   2.292 +        v-roll (view-image)
   2.293          v-pitch (view-image)
   2.294 -        v-roll (view-image)
   2.295          ]
   2.296      (fn [prop-data]
   2.297        (dorun
   2.298         (map
   2.299 -        (fn [[p y r]]
   2.300 +        (fn [[y r p]]
   2.301            (v-yaw (yaw y))
   2.302            (v-roll (roll r))
   2.303            (v-pitch (pitch p)))
   2.304          prop-data)))))
   2.305 -       
   2.306 +(comment  
   2.307 +     
   2.308 +(defn proprioception-debug-window
   2.309 +  []
   2.310 +  (let [time (atom 0)]
   2.311 +    (fn [prop-data]
   2.312 +      (if (= 0 (rem (swap! time inc) 40))
   2.313 +        (println-repl prop-data)))))
   2.314 +)
   2.315 +
   2.316  (comment
   2.317    (dorun
   2.318     (map
   2.319 @@ -439,8 +477,11 @@
   2.320                       :mass 0 :color ColorRGBA/Green :name "hand")
   2.321          finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0)
   2.322                      :mass 1 :color ColorRGBA/Red :name "finger")
   2.323 -        floor   (box 10 0.5 10 :position (Vector3f. 0 -5 0)
   2.324 +        floor   (box 10 10 10 :position (Vector3f. 0 -15 0)
   2.325                       :mass 0 :color ColorRGBA/Gray)
   2.326 +        joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow
   2.327 +                        :position (Vector3f. 1.2 2 0)
   2.328 +                        :physical? false)
   2.329  
   2.330          move-up? (atom false)
   2.331          move-down? (atom false)
   2.332 @@ -451,8 +492,8 @@
   2.333          control (.getControl finger RigidBodyControl)
   2.334          time (atom 0)
   2.335          joint (join-at-point hand finger (Vector3f. 1.2 2 0 ))
   2.336 -        creature (nodify [hand finger])
   2.337 -        prop (proprioception creature)
   2.338 +        creature (nodify [hand finger joint-node])
   2.339 +        prop (joint-proprioception creature joint-node)
   2.340  
   2.341          prop-view (proprioception-debug-window)
   2.342  
   2.343 @@ -461,13 +502,14 @@
   2.344      
   2.345  
   2.346      
   2.347 +
   2.348      (.setCollisionGroup
   2.349       (.getControl hand RigidBodyControl)
   2.350       PhysicsCollisionObject/COLLISION_GROUP_NONE)
   2.351  
   2.352      
   2.353      (world
   2.354 -     (nodify [hand finger floor])
   2.355 +     (nodify [hand finger floor joint-node])
   2.356       (merge standard-debug-controls
   2.357              {"key-r" (fn [_ pressed?] (reset! move-up? pressed?))
   2.358               "key-t" (fn [_ pressed?] (reset! move-down? pressed?))
   2.359 @@ -506,7 +548,7 @@
   2.360                                (Vector3f. 1 0 0))))
   2.361  
   2.362         ;;(if (= 0 (rem (swap! time inc) 20))
   2.363 -         (prop-view (prop))))))
   2.364 +         (prop-view (list (prop)))))))
   2.365  
   2.366  #+end_src
   2.367  

     3.1 --- a/org/test-creature.org	Tue Jan 31 01:40:47 2012 -0700
     3.2 +++ b/org/test-creature.org	Wed Feb 01 02:27:18 2012 -0700
     3.3 @@ -297,6 +297,41 @@
     3.4             (take 2 targets))
     3.5            (recur (float (* radius 2))))))))
     3.6  
     3.7 +
     3.8 +(defn proprio-joint [#^Node parts #^Node joint]
     3.9 +  (let [[obj-a obj-b] (joint-targets parts joint)
    3.10 +        joint-rot (.getWorldRotation joint)
    3.11 +        x (.mult joint-rot Vector3f/UNIT_X)
    3.12 +        y (.mult joint-rot Vector3f/UNIT_Y)
    3.13 +        z (.mult joint-rot Vector3f/UNIT_Z)]
    3.14 +    ;; this function will report proprioceptive information for the
    3.15 +    ;; joint 
    3.16 +    (fn []
    3.17 +      ;; x is the "twist" axis, y and z are the "bend" axes
    3.18 +      (let [rot-a (.getWorldRotation obj-a)
    3.19 +            rot-b (.getWorldRotation obj-b)
    3.20 +            relative (.mult (.inverse rot-a) rot-b)
    3.21 +            basis (doto (Matrix3f.)
    3.22 +                    (.setColumn 0 y)
    3.23 +                    (.setColumn 1 z)
    3.24 +                    (.setColumn 2 x))
    3.25 +            rotation-about-joint
    3.26 +            (doto (Quaternion.)
    3.27 +              (.fromRotationMatrix 
    3.28 +               (.mult (.inverse basis)
    3.29 +                      (.toRotationMatrix relative))))
    3.30 +
    3.31 +            confirm-axes
    3.32 +            (let [temp-axes (make-array Vector3f 3)]
    3.33 +              (.toAxes rotation-about-joint temp-axes)
    3.34 +              (seq temp-axes))
    3.35 +            euler-angles
    3.36 +            (seq (.toAngles rotation-about-joint nil))]
    3.37 +        ;;return euler angles of the quaternion around the new basis            
    3.38 +        euler-angles))))
    3.39 +
    3.40 +
    3.41 +
    3.42  (defn world-to-local
    3.43     "Convert the world coordinates into coordinates relative to the 
    3.44     object (i.e. local coordinates), taking into account the rotation
    3.45 @@ -457,8 +492,7 @@
    3.46    (dorun
    3.47     (map
    3.48      (fn [joint]
    3.49 -      (let [[obj-a obj-b]
    3.50 -            (joint-targets pieces joint)]
    3.51 +      (let [[obj-a obj-b] (joint-targets pieces joint)]
    3.52          (connect obj-a obj-b joint)))
    3.53      joints))
    3.54    pieces)
    3.55 @@ -1188,7 +1222,7 @@
    3.56                (comp (view-image) BufferedImage!))
    3.57       
    3.58       (add-eye world (.getCamera world) no-op)
    3.59 -     
    3.60 +     ;;(set-gravity world (Vector3f. 0 0 0))
    3.61       ;;(com.aurellem.capture.Capture/captureVideo
    3.62       ;; world (file-str "/home/r/proj/ai-videos/hand"))
    3.63       ;;(.setTimer world (RatchetTimer. 60))
    3.64 @@ -1323,7 +1357,7 @@
    3.65  #+end_src
    3.66  
    3.67  #+results: body-1
    3.68 -: #'cortex.silly/test-creature
    3.69 +: #'cortex.silly/follow-test
    3.70  
    3.71  
    3.72  * COMMENT purgatory