1.1 --- a/org/body.org	Tue Jan 31 01:40:47 2012 -0700
     1.2 +++ b/org/body.org	Wed Feb 01 02:27:18 2012 -0700
     1.3 @@ -17,67 +17,8 @@
     1.4     com.jme3.math.Matrix3f
     1.5     com.jme3.bullet.control.RigidBodyControl))
     1.6  
     1.7 -
     1.8 -
     1.9 -(defn quaternion-decompose [#^Quaternion q]
    1.10 -  (map
    1.11 -   #(arc-between % (.rotate q %))
    1.12 -   [Vector3f/UNIT_X
    1.13 -    Vector3f/UNIT_Y
    1.14 -    Vector3f/UNIT_Z]))
    1.15 -
    1.16 -(defn any-orthogonal
    1.17 -  "Generate an arbitray (but stable) orthogonal vector to a given
    1.18 -   vector."
    1.19 -  [vector]
    1.20 -  (let [x (.getX vector)
    1.21 -        y (.getY vector)
    1.22 -        z (.getZ vector)]
    1.23 -    (cond
    1.24 -     (not= x (float 0)) (Vector3f. (- z) 0 x)
    1.25 -     (not= y (float 0)) (Vector3f. 0 (- z) y)
    1.26 -     (not= z (float 0)) (Vector3f. 0 (- z) y)
    1.27 -     true Vector3f/ZERO)))
    1.28 -
    1.29 -(defn project-quaternion
    1.30 -  "From http://stackoverflow.com/questions/3684269/
    1.31 -   component-of-a-quaternion-rotation-around-an-axis.
    1.32 -
    1.33 -   Determine the amount of rotation a quaternion will
    1.34 -   cause about a given axis."
    1.35 -  [#^Quaternion q #^Vector3f axis]
    1.36 -  (let [basis-1 (any-orthogonal axis)
    1.37 -        basis-2 (.cross axis basis-1)
    1.38 -        rotated (.mult q basis-1)
    1.39 -        alpha (.dot basis-1 (.project rotated basis-1))
    1.40 -        beta  (.dot basis-2 (.project rotated basis-2))]
    1.41 -    (Math/atan2 beta alpha)))
    1.42 -
    1.43 -(defn right-handed? [vec1 vec2 vec3]
    1.44 -  (< 0 (.dot (.cross vec1 vec2) vec3)))
    1.45 -
    1.46 -(defn project-quaternion
    1.47 -  "From http://stackoverflow.com/questions/3684269/
    1.48 -   component-of-a-quaternion-rotation-around-an-axis.
    1.49 -
    1.50 -   Determine the amount of rotation a quaternion will
    1.51 -   cause about a given axis."
    1.52 -  [#^Quaternion q #^Vector3f axis]
    1.53 -  (let [axis (.normalize axis)
    1.54 -        basis-1 (any-orthogonal axis)
    1.55 -        basis-2 (.cross axis basis-1)
    1.56 -        rotated (.mult q basis-1)
    1.57 -        rotated-in-plane (.add (.project rotated basis-1)
    1.58 -                               (.project rotated basis-2))]
    1.59 -
    1.60 -    ;; be sure to get sign from cross product
    1.61 -    (if (right-handed? basis-1 rotated-in-plane axis)
    1.62 -      (.angleBetween rotated-in-plane basis-1)
    1.63 -      (- (* 2 Math/PI) (.angleBetween rotated-in-plane basis-1)))))
    1.64 -
    1.65 -
    1.66 -
    1.67 -(defn joint-proprioception
    1.68 +(comment
    1.69 +  (defn joint-proprioception
    1.70    "Relative position information for a two-part system connected by a
    1.71     joint. Gives the pitch, yaw, and roll of the 'B' object relative to
    1.72     the 'A' object, as determined by the joint."
    1.73 @@ -120,63 +61,148 @@
    1.74           arm-b)]
    1.75      ;;(println-repl (.getName object-a) (.getName object-b))
    1.76      [pitch yaw roll]))
    1.77 +)
    1.78  
    1.79 +(defn any-orthogonal
    1.80 +  "Generate an arbitray (but stable) orthogonal vector to a given
    1.81 +   vector."
    1.82 +  [vector]
    1.83 +  (let [x (.getX vector)
    1.84 +        y (.getY vector)
    1.85 +        z (.getZ vector)]
    1.86 +    (cond
    1.87 +     (not= x (float 0)) (Vector3f. (- z) 0 x)
    1.88 +     (not= y (float 0)) (Vector3f. 0 (- z) y)
    1.89 +     (not= z (float 0)) (Vector3f. 0 (- z) y)
    1.90 +     true Vector3f/ZERO)))
    1.91  
    1.92 +(comment
    1.93 +(defn project-quaternion
    1.94 +  "From http://stackoverflow.com/questions/3684269/
    1.95 +   component-of-a-quaternion-rotation-around-an-axis.
    1.96  
    1.97 +   Determine the amount of rotation a quaternion will
    1.98 +   cause about a given axis."
    1.99 +  [#^Quaternion q #^Vector3f axis]
   1.100 +  (let [basis-1 (any-orthogonal axis)
   1.101 +        basis-2 (.cross axis basis-1)
   1.102 +        rotated (.mult q basis-1)
   1.103 +        alpha (.dot basis-1 (.project rotated basis-1))
   1.104 +        beta  (.dot basis-2 (.project rotated basis-2))]
   1.105 +    (Math/atan2 beta alpha)))
   1.106 +)
   1.107  
   1.108 -(defn absolute-angle-between
   1.109 -  [vec-1 vec-2]
   1.110 +(defn right-handed? [vec1 vec2 vec3]
   1.111 +  (< 0 (.dot (.cross vec1 vec2) vec3)))
   1.112 +
   1.113 +(defn absolute-angle [vec1 vec2 axis]
   1.114 +  (let [angle (.angleBetween vec1 vec2)]
   1.115 +    (if (right-handed? vec1 vec2 axis)
   1.116 +      angle (- (* 2 Math/PI) angle))))
   1.117 +
   1.118 +(defn angle-min [& angles]
   1.119 +  (first
   1.120 +   (sort-by 
   1.121 +    (fn [angle]
   1.122 +      (let [in-circle (Math/abs (rem angle (* 2 Math/PI)))]
   1.123 +        (min in-circle
   1.124 +             (- (* Math/PI 2) in-circle))))
   1.125 +    angles)))
   1.126 +
   1.127 +(defn project-quaternion
   1.128 +  "From http://stackoverflow.com/questions/3684269/
   1.129 +   component-of-a-quaternion-rotation-around-an-axis.
   1.130 +
   1.131 +   Determine the amount of rotation a quaternion will
   1.132 +   cause about a given axis."
   1.133 +  [#^Quaternion q #^Vector3f axis]
   1.134 +  (let [axis (.normalize axis)
   1.135 +        basis-1 (.normalize (any-orthogonal axis))
   1.136 +        basis-2 (.cross axis basis-1)
   1.137 +        rotated-1 (.mult q basis-1)
   1.138 +        basis-1* (.normalize
   1.139 +                          (.add (.project rotated-1 basis-1)
   1.140 +                                (.project rotated-1 basis-2)))
   1.141 +        rotated-2 (.mult q basis-2)
   1.142 +        basis-2* (.normalize
   1.143 +                          (.add (.project rotated-2 basis-1)
   1.144 +                                (.project rotated-2 basis-2)))
   1.145 +        angle-1
   1.146 +        (absolute-angle basis-1 basis-1* axis)
   1.147 +        angle-2
   1.148 +        (absolute-angle basis-2 basis-2* axis)
   1.149 +
   1.150 +
   1.151 +        angle (angle-min angle-1 angle-2)
   1.152 +        ]
   1.153    
   1.154  
   1.155 +    ;; be sure to get sign from cross product
   1.156 +    (if false
   1.157 +      (do
   1.158 +        (println-repl "axis" axis)
   1.159 +        (println-repl "basis-1" basis-1)
   1.160 +        (println-repl "basis-2" basis-2)
   1.161 +        (println-repl "rotated-1" rotated-1)
   1.162 +        (println-repl "rotated-2" rotated-2)
   1.163 +        (println-repl "basis-1*" basis-1*)
   1.164 +        (println-repl "basis-2*" basis-2*)
   1.165 +        (println-repl "angle-1" angle-1)
   1.166 +        (println-repl "angle-2" angle-2)
   1.167 +        
   1.168 +        (println-repl "angle" angle)
   1.169 +        (println-repl "")))
   1.170 +    angle))
   1.171 +    
   1.172  
   1.173 +(import com.jme3.scene.Node)
   1.174 +
   1.175 +(defn joint-proprioception [#^Node parts #^Node joint]
   1.176 +  (let [[obj-a obj-b] (cortex.silly/joint-targets parts joint)
   1.177 +        joint-rot (.getWorldRotation joint)
   1.178 +        x (.mult joint-rot Vector3f/UNIT_X)
   1.179 +        y (.mult joint-rot Vector3f/UNIT_Y)
   1.180 +        z (.mult joint-rot Vector3f/UNIT_Z)]
   1.181 +    ;; this function will report proprioceptive information for the
   1.182 +    ;; joint 
   1.183 +    (fn []
   1.184 +      ;; x is the "twist" axis, y and z are the "bend" axes
   1.185 +      (let [rot-a (.getWorldRotation obj-a)
   1.186 +            rot-b (.getWorldRotation obj-b)
   1.187 +            relative (.mult (.inverse rot-a) rot-b)
   1.188 +            basis (doto (Matrix3f.)
   1.189 +                    (.setColumn 0 x)
   1.190 +                    (.setColumn 1 y)
   1.191 +                    (.setColumn 2 z))
   1.192 +            rotation-about-joint
   1.193 +            (doto (Quaternion.)
   1.194 +              (.fromRotationMatrix 
   1.195 +               (.mult (.invert basis)
   1.196 +                      (.toRotationMatrix relative))))
   1.197 +            [yaw roll pitch]
   1.198 +            (seq (.toAngles rotation-about-joint nil))]
   1.199 +        ;;return euler angles of the quaternion around the new basis            
   1.200 +        ;;[yaw pitch roll]
   1.201 +        [yaw roll pitch]
   1.202 +        ))))
   1.203 +
   1.204 +
   1.205 +(comment
   1.206  
   1.207  (defn joint-proprioception
   1.208    [joint]
   1.209    (let [object-a (.getUserObject (.getBodyA joint))
   1.210          object-b (.getUserObject (.getBodyB joint))
   1.211 -
   1.212 -        arm-a
   1.213 -        (.normalize
   1.214 -         (.subtract 
   1.215 -          (.localToWorld object-a (.getPivotA joint) nil)
   1.216 -          (.getWorldTranslation object-a)))
   1.217 -        arm-b
   1.218 -        (.normalize
   1.219 -         (.subtract 
   1.220 -          (.localToWorld object-b (.getPivotB joint) nil)
   1.221 -          (.getWorldTranslation object-b)))
   1.222 -
   1.223 -        rotate-a (.clone (.getWorldRotation object-a))
   1.224 -        unrotate-a (.inverse (.getWorldRotation object-a))
   1.225 -
   1.226 -        canonical-arm-a (.mult unrotate-a arm-a)
   1.227 -
   1.228 -        basis-1 (any-orthogonal canonical-arm-a)
   1.229 -        basis-2 (.normalize (.cross basis-1 canonical-arm-a))
   1.230 -
   1.231 -        pitch (.angleBetween arm-b basis-1)
   1.232 -        yaw (.angleBetween arm-b basis-2)
   1.233 -
   1.234 -        twist-a
   1.235 -        (project-quaternion
   1.236 -         (.getWorldRotation object-a) arm-a)
   1.237 -        
   1.238 -        twist-b
   1.239 -        (project-quaternion
   1.240 -         (.getWorldRotation object-b) arm-b)
   1.241 -
   1.242 -        roll (- twist-b 0)
   1.243 -        ;; "un-rotate" arm-a to it's canonical position, get two
   1.244 -        ;; orthogonal basis vectors. Rotate those two vectors back to
   1.245 -        ;; the correct position get the rotations between them.
   1.246 -
   1.247 -        ;; twist is the rotation about arm-a of obj-b minus the
   1.248 -        ;; rotation about arm-a of obj-a
   1.249 +        rot-a (.clone (.getWorldRotation object-a))
   1.250 +        rot-b (.clone (.getWorldRotation object-b))
   1.251          ]
   1.252 +    
   1.253 +    (.mult rot-b (.inverse rot-a))
   1.254 +    
   1.255      ;; object-a == hand
   1.256      ;; object-b == finger
   1.257 -    [pitch yaw roll]))
   1.258 -
   1.259 +    ))
   1.260 +)
   1.261  ;; (defn joint-proprioception*
   1.262  ;;   [joint]
   1.263  ;;   (let [object-a (.getUserObject (.getBodyA joint))
   1.264 @@ -211,6 +237,9 @@
   1.265    
   1.266  #+end_src
   1.267  
   1.268 +#+results: proprioception
   1.269 +: #'cortex.body/proprioception
   1.270 +
   1.271  * Motor Control
   1.272  #+name: motor-control
   1.273  #+begin_src clojure
   1.274 @@ -346,7 +375,7 @@
   1.275     nil
   1.276     ))
   1.277  
   1.278 -
   1.279 +(import com.jme3.bullet.collision.PhysicsCollisionObject)
   1.280  
   1.281  (defn blab-* []
   1.282    (let [hand   (box 0.5 0.2 0.2 :position (Vector3f. 0 0 0)
   1.283 @@ -397,21 +426,30 @@
   1.284  (defn proprioception-debug-window
   1.285    []
   1.286    (let [yaw (view-angle 0xFF0000)
   1.287 -        pitch (view-angle 0x00FF00)
   1.288 -        roll (view-angle 0xFFFFFF)
   1.289 +        roll (view-angle 0x00FF00)
   1.290 +        pitch (view-angle 0xFFFFFF)
   1.291          v-yaw (view-image)
   1.292 +        v-roll (view-image)
   1.293          v-pitch (view-image)
   1.294 -        v-roll (view-image)
   1.295          ]
   1.296      (fn [prop-data]
   1.297        (dorun
   1.298         (map
   1.299 -        (fn [[p y r]]
   1.300 +        (fn [[y r p]]
   1.301            (v-yaw (yaw y))
   1.302            (v-roll (roll r))
   1.303            (v-pitch (pitch p)))
   1.304          prop-data)))))
   1.305 -       
   1.306 +(comment  
   1.307 +     
   1.308 +(defn proprioception-debug-window
   1.309 +  []
   1.310 +  (let [time (atom 0)]
   1.311 +    (fn [prop-data]
   1.312 +      (if (= 0 (rem (swap! time inc) 40))
   1.313 +        (println-repl prop-data)))))
   1.314 +)
   1.315 +
   1.316  (comment
   1.317    (dorun
   1.318     (map
   1.319 @@ -439,8 +477,11 @@
   1.320                       :mass 0 :color ColorRGBA/Green :name "hand")
   1.321          finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0)
   1.322                      :mass 1 :color ColorRGBA/Red :name "finger")
   1.323 -        floor   (box 10 0.5 10 :position (Vector3f. 0 -5 0)
   1.324 +        floor   (box 10 10 10 :position (Vector3f. 0 -15 0)
   1.325                       :mass 0 :color ColorRGBA/Gray)
   1.326 +        joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow
   1.327 +                        :position (Vector3f. 1.2 2 0)
   1.328 +                        :physical? false)
   1.329  
   1.330          move-up? (atom false)
   1.331          move-down? (atom false)
   1.332 @@ -451,8 +492,8 @@
   1.333          control (.getControl finger RigidBodyControl)
   1.334          time (atom 0)
   1.335          joint (join-at-point hand finger (Vector3f. 1.2 2 0 ))
   1.336 -        creature (nodify [hand finger])
   1.337 -        prop (proprioception creature)
   1.338 +        creature (nodify [hand finger joint-node])
   1.339 +        prop (joint-proprioception creature joint-node)
   1.340  
   1.341          prop-view (proprioception-debug-window)
   1.342  
   1.343 @@ -461,13 +502,14 @@
   1.344      
   1.345  
   1.346      
   1.347 +
   1.348      (.setCollisionGroup
   1.349       (.getControl hand RigidBodyControl)
   1.350       PhysicsCollisionObject/COLLISION_GROUP_NONE)
   1.351  
   1.352      
   1.353      (world
   1.354 -     (nodify [hand finger floor])
   1.355 +     (nodify [hand finger floor joint-node])
   1.356       (merge standard-debug-controls
   1.357              {"key-r" (fn [_ pressed?] (reset! move-up? pressed?))
   1.358               "key-t" (fn [_ pressed?] (reset! move-down? pressed?))
   1.359 @@ -506,7 +548,7 @@
   1.360                                (Vector3f. 1 0 0))))
   1.361  
   1.362         ;;(if (= 0 (rem (swap! time inc) 20))
   1.363 -         (prop-view (prop))))))
   1.364 +         (prop-view (list (prop)))))))
   1.365  
   1.366  #+end_src
   1.367