# HG changeset patch # User Robert McIntyre # Date 1328088438 25200 # Node ID 2ed7e60d3821024c0f79f8c30a12d460802d414c # Parent 3206d5e20beeeb865a9e09bcb15c10a44dc52056 FINALLY got proprioception working diff -r 3206d5e20bee -r 2ed7e60d3821 assets/Models/creature1/try-again.blend Binary file assets/Models/creature1/try-again.blend has changed diff -r 3206d5e20bee -r 2ed7e60d3821 org/body.org --- a/org/body.org Tue Jan 31 01:40:47 2012 -0700 +++ b/org/body.org Wed Feb 01 02:27:18 2012 -0700 @@ -17,67 +17,8 @@ com.jme3.math.Matrix3f com.jme3.bullet.control.RigidBodyControl)) - - -(defn quaternion-decompose [#^Quaternion q] - (map - #(arc-between % (.rotate q %)) - [Vector3f/UNIT_X - Vector3f/UNIT_Y - Vector3f/UNIT_Z])) - -(defn any-orthogonal - "Generate an arbitray (but stable) orthogonal vector to a given - vector." - [vector] - (let [x (.getX vector) - y (.getY vector) - z (.getZ vector)] - (cond - (not= x (float 0)) (Vector3f. (- z) 0 x) - (not= y (float 0)) (Vector3f. 0 (- z) y) - (not= z (float 0)) (Vector3f. 0 (- z) y) - true Vector3f/ZERO))) - -(defn project-quaternion - "From http://stackoverflow.com/questions/3684269/ - component-of-a-quaternion-rotation-around-an-axis. - - Determine the amount of rotation a quaternion will - cause about a given axis." - [#^Quaternion q #^Vector3f axis] - (let [basis-1 (any-orthogonal axis) - basis-2 (.cross axis basis-1) - rotated (.mult q basis-1) - alpha (.dot basis-1 (.project rotated basis-1)) - beta (.dot basis-2 (.project rotated basis-2))] - (Math/atan2 beta alpha))) - -(defn right-handed? [vec1 vec2 vec3] - (< 0 (.dot (.cross vec1 vec2) vec3))) - -(defn project-quaternion - "From http://stackoverflow.com/questions/3684269/ - component-of-a-quaternion-rotation-around-an-axis. - - Determine the amount of rotation a quaternion will - cause about a given axis." - [#^Quaternion q #^Vector3f axis] - (let [axis (.normalize axis) - basis-1 (any-orthogonal axis) - basis-2 (.cross axis basis-1) - rotated (.mult q basis-1) - rotated-in-plane (.add (.project rotated basis-1) - (.project rotated basis-2))] - - ;; be sure to get sign from cross product - (if (right-handed? basis-1 rotated-in-plane axis) - (.angleBetween rotated-in-plane basis-1) - (- (* 2 Math/PI) (.angleBetween rotated-in-plane basis-1))))) - - - -(defn joint-proprioception +(comment + (defn joint-proprioception "Relative position information for a two-part system connected by a joint. Gives the pitch, yaw, and roll of the 'B' object relative to the 'A' object, as determined by the joint." @@ -120,63 +61,148 @@ arm-b)] ;;(println-repl (.getName object-a) (.getName object-b)) [pitch yaw roll])) +) +(defn any-orthogonal + "Generate an arbitray (but stable) orthogonal vector to a given + vector." + [vector] + (let [x (.getX vector) + y (.getY vector) + z (.getZ vector)] + (cond + (not= x (float 0)) (Vector3f. (- z) 0 x) + (not= y (float 0)) (Vector3f. 0 (- z) y) + (not= z (float 0)) (Vector3f. 0 (- z) y) + true Vector3f/ZERO))) +(comment +(defn project-quaternion + "From http://stackoverflow.com/questions/3684269/ + component-of-a-quaternion-rotation-around-an-axis. + Determine the amount of rotation a quaternion will + cause about a given axis." + [#^Quaternion q #^Vector3f axis] + (let [basis-1 (any-orthogonal axis) + basis-2 (.cross axis basis-1) + rotated (.mult q basis-1) + alpha (.dot basis-1 (.project rotated basis-1)) + beta (.dot basis-2 (.project rotated basis-2))] + (Math/atan2 beta alpha))) +) -(defn absolute-angle-between - [vec-1 vec-2] +(defn right-handed? [vec1 vec2 vec3] + (< 0 (.dot (.cross vec1 vec2) vec3))) + +(defn absolute-angle [vec1 vec2 axis] + (let [angle (.angleBetween vec1 vec2)] + (if (right-handed? vec1 vec2 axis) + angle (- (* 2 Math/PI) angle)))) + +(defn angle-min [& angles] + (first + (sort-by + (fn [angle] + (let [in-circle (Math/abs (rem angle (* 2 Math/PI)))] + (min in-circle + (- (* Math/PI 2) in-circle)))) + angles))) + +(defn project-quaternion + "From http://stackoverflow.com/questions/3684269/ + component-of-a-quaternion-rotation-around-an-axis. + + Determine the amount of rotation a quaternion will + cause about a given axis." + [#^Quaternion q #^Vector3f axis] + (let [axis (.normalize axis) + basis-1 (.normalize (any-orthogonal axis)) + basis-2 (.cross axis basis-1) + rotated-1 (.mult q basis-1) + basis-1* (.normalize + (.add (.project rotated-1 basis-1) + (.project rotated-1 basis-2))) + rotated-2 (.mult q basis-2) + basis-2* (.normalize + (.add (.project rotated-2 basis-1) + (.project rotated-2 basis-2))) + angle-1 + (absolute-angle basis-1 basis-1* axis) + angle-2 + (absolute-angle basis-2 basis-2* axis) + + + angle (angle-min angle-1 angle-2) + ] + ;; be sure to get sign from cross product + (if false + (do + (println-repl "axis" axis) + (println-repl "basis-1" basis-1) + (println-repl "basis-2" basis-2) + (println-repl "rotated-1" rotated-1) + (println-repl "rotated-2" rotated-2) + (println-repl "basis-1*" basis-1*) + (println-repl "basis-2*" basis-2*) + (println-repl "angle-1" angle-1) + (println-repl "angle-2" angle-2) + + (println-repl "angle" angle) + (println-repl ""))) + angle)) + +(import com.jme3.scene.Node) + +(defn joint-proprioception [#^Node parts #^Node joint] + (let [[obj-a obj-b] (cortex.silly/joint-targets parts joint) + joint-rot (.getWorldRotation joint) + x (.mult joint-rot Vector3f/UNIT_X) + y (.mult joint-rot Vector3f/UNIT_Y) + z (.mult joint-rot Vector3f/UNIT_Z)] + ;; this function will report proprioceptive information for the + ;; joint + (fn [] + ;; x is the "twist" axis, y and z are the "bend" axes + (let [rot-a (.getWorldRotation obj-a) + rot-b (.getWorldRotation obj-b) + relative (.mult (.inverse rot-a) rot-b) + basis (doto (Matrix3f.) + (.setColumn 0 x) + (.setColumn 1 y) + (.setColumn 2 z)) + rotation-about-joint + (doto (Quaternion.) + (.fromRotationMatrix + (.mult (.invert basis) + (.toRotationMatrix relative)))) + [yaw roll pitch] + (seq (.toAngles rotation-about-joint nil))] + ;;return euler angles of the quaternion around the new basis + ;;[yaw pitch roll] + [yaw roll pitch] + )))) + + +(comment (defn joint-proprioception [joint] (let [object-a (.getUserObject (.getBodyA joint)) object-b (.getUserObject (.getBodyB joint)) - - arm-a - (.normalize - (.subtract - (.localToWorld object-a (.getPivotA joint) nil) - (.getWorldTranslation object-a))) - arm-b - (.normalize - (.subtract - (.localToWorld object-b (.getPivotB joint) nil) - (.getWorldTranslation object-b))) - - rotate-a (.clone (.getWorldRotation object-a)) - unrotate-a (.inverse (.getWorldRotation object-a)) - - canonical-arm-a (.mult unrotate-a arm-a) - - basis-1 (any-orthogonal canonical-arm-a) - basis-2 (.normalize (.cross basis-1 canonical-arm-a)) - - pitch (.angleBetween arm-b basis-1) - yaw (.angleBetween arm-b basis-2) - - twist-a - (project-quaternion - (.getWorldRotation object-a) arm-a) - - twist-b - (project-quaternion - (.getWorldRotation object-b) arm-b) - - roll (- twist-b 0) - ;; "un-rotate" arm-a to it's canonical position, get two - ;; orthogonal basis vectors. Rotate those two vectors back to - ;; the correct position get the rotations between them. - - ;; twist is the rotation about arm-a of obj-b minus the - ;; rotation about arm-a of obj-a + rot-a (.clone (.getWorldRotation object-a)) + rot-b (.clone (.getWorldRotation object-b)) ] + + (.mult rot-b (.inverse rot-a)) + ;; object-a == hand ;; object-b == finger - [pitch yaw roll])) - + )) +) ;; (defn joint-proprioception* ;; [joint] ;; (let [object-a (.getUserObject (.getBodyA joint)) @@ -211,6 +237,9 @@ #+end_src +#+results: proprioception +: #'cortex.body/proprioception + * Motor Control #+name: motor-control #+begin_src clojure @@ -346,7 +375,7 @@ nil )) - +(import com.jme3.bullet.collision.PhysicsCollisionObject) (defn blab-* [] (let [hand (box 0.5 0.2 0.2 :position (Vector3f. 0 0 0) @@ -397,21 +426,30 @@ (defn proprioception-debug-window [] (let [yaw (view-angle 0xFF0000) - pitch (view-angle 0x00FF00) - roll (view-angle 0xFFFFFF) + roll (view-angle 0x00FF00) + pitch (view-angle 0xFFFFFF) v-yaw (view-image) + v-roll (view-image) v-pitch (view-image) - v-roll (view-image) ] (fn [prop-data] (dorun (map - (fn [[p y r]] + (fn [[y r p]] (v-yaw (yaw y)) (v-roll (roll r)) (v-pitch (pitch p))) prop-data))))) - +(comment + +(defn proprioception-debug-window + [] + (let [time (atom 0)] + (fn [prop-data] + (if (= 0 (rem (swap! time inc) 40)) + (println-repl prop-data))))) +) + (comment (dorun (map @@ -439,8 +477,11 @@ :mass 0 :color ColorRGBA/Green :name "hand") finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0) :mass 1 :color ColorRGBA/Red :name "finger") - floor (box 10 0.5 10 :position (Vector3f. 0 -5 0) + floor (box 10 10 10 :position (Vector3f. 0 -15 0) :mass 0 :color ColorRGBA/Gray) + joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow + :position (Vector3f. 1.2 2 0) + :physical? false) move-up? (atom false) move-down? (atom false) @@ -451,8 +492,8 @@ control (.getControl finger RigidBodyControl) time (atom 0) joint (join-at-point hand finger (Vector3f. 1.2 2 0 )) - creature (nodify [hand finger]) - prop (proprioception creature) + creature (nodify [hand finger joint-node]) + prop (joint-proprioception creature joint-node) prop-view (proprioception-debug-window) @@ -461,13 +502,14 @@ + (.setCollisionGroup (.getControl hand RigidBodyControl) PhysicsCollisionObject/COLLISION_GROUP_NONE) (world - (nodify [hand finger floor]) + (nodify [hand finger floor joint-node]) (merge standard-debug-controls {"key-r" (fn [_ pressed?] (reset! move-up? pressed?)) "key-t" (fn [_ pressed?] (reset! move-down? pressed?)) @@ -506,7 +548,7 @@ (Vector3f. 1 0 0)))) ;;(if (= 0 (rem (swap! time inc) 20)) - (prop-view (prop)))))) + (prop-view (list (prop))))))) #+end_src diff -r 3206d5e20bee -r 2ed7e60d3821 org/test-creature.org --- a/org/test-creature.org Tue Jan 31 01:40:47 2012 -0700 +++ b/org/test-creature.org Wed Feb 01 02:27:18 2012 -0700 @@ -297,6 +297,41 @@ (take 2 targets)) (recur (float (* radius 2)))))))) + +(defn proprio-joint [#^Node parts #^Node joint] + (let [[obj-a obj-b] (joint-targets parts joint) + joint-rot (.getWorldRotation joint) + x (.mult joint-rot Vector3f/UNIT_X) + y (.mult joint-rot Vector3f/UNIT_Y) + z (.mult joint-rot Vector3f/UNIT_Z)] + ;; this function will report proprioceptive information for the + ;; joint + (fn [] + ;; x is the "twist" axis, y and z are the "bend" axes + (let [rot-a (.getWorldRotation obj-a) + rot-b (.getWorldRotation obj-b) + relative (.mult (.inverse rot-a) rot-b) + basis (doto (Matrix3f.) + (.setColumn 0 y) + (.setColumn 1 z) + (.setColumn 2 x)) + rotation-about-joint + (doto (Quaternion.) + (.fromRotationMatrix + (.mult (.inverse basis) + (.toRotationMatrix relative)))) + + confirm-axes + (let [temp-axes (make-array Vector3f 3)] + (.toAxes rotation-about-joint temp-axes) + (seq temp-axes)) + euler-angles + (seq (.toAngles rotation-about-joint nil))] + ;;return euler angles of the quaternion around the new basis + euler-angles)))) + + + (defn world-to-local "Convert the world coordinates into coordinates relative to the object (i.e. local coordinates), taking into account the rotation @@ -457,8 +492,7 @@ (dorun (map (fn [joint] - (let [[obj-a obj-b] - (joint-targets pieces joint)] + (let [[obj-a obj-b] (joint-targets pieces joint)] (connect obj-a obj-b joint))) joints)) pieces) @@ -1188,7 +1222,7 @@ (comp (view-image) BufferedImage!)) (add-eye world (.getCamera world) no-op) - + ;;(set-gravity world (Vector3f. 0 0 0)) ;;(com.aurellem.capture.Capture/captureVideo ;; world (file-str "/home/r/proj/ai-videos/hand")) ;;(.setTimer world (RatchetTimer. 60)) @@ -1323,7 +1357,7 @@ #+end_src #+results: body-1 -: #'cortex.silly/test-creature +: #'cortex.silly/follow-test * COMMENT purgatory