#+title: The BODY!!!

 #+author: Robert McIntyre

 #+email: rlm@mit.edu

 #+description: Simulating a body (movement, touch, propioception) in jMonkeyEngine3.

 #+SETUPFILE: ../../aurellem/org/setup.org

 #+INCLUDE: ../../aurellem/org/level-0.org

 

 * Proprioception

 #+name: proprioception

 #+begin_src clojure 

 (ns cortex.body

   (:use (cortex world util))

   (:import

    com.jme3.math.Vector3f

    com.jme3.math.Quaternion

    com.jme3.math.Vector2f

    com.jme3.math.Matrix3f

    com.jme3.bullet.control.RigidBodyControl))

 

 (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 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."

   [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)))

         rotate-a

          (doto (Matrix3f.)

            (.fromStartEndVectors arm-a Vector3f/UNIT_X))

         arm-b

         (.mult

          rotate-a 

          (.normalize

           (.subtract 

            (.localToWorld object-b (.getPivotB joint) nil)

            (.getWorldTranslation object-b))))

         pitch

         (.angleBetween

          (.normalize (Vector2f. (.getX arm-b) (.getY arm-b)))

          (Vector2f. 1 0))

         yaw

         (.angleBetween

          (.normalize (Vector2f. (.getX arm-b) (.getZ arm-b)))

          (Vector2f. 1 0))

                      

         roll

         (project-quaternion

          (.mult 

           (.getLocalRotation object-b)

           (doto (Quaternion.)

             (.fromRotationMatrix rotate-a)))

          arm-b)]

     [pitch yaw roll]))

 

 (defn proprioception

   "Create a function that provides proprioceptive information about an

   entire body."

   [body]

   ;; extract the body's joints

   (let [joints 

         (distinct

          (reduce

           concat

           (map #(.getJoints %)

                (keep

                 #(.getControl % RigidBodyControl)

                 (node-seq body)))))]

     (fn []

       (map joint-proprioception joints))))

   

 #+end_src

 

 * Motor Control

 #+name: motor-control

 #+begin_src clojure

 (in-ns 'cortex.body)

 

 ;; surprisingly enough, terristerial creatures only move by using

 ;; torque applied about their joints.  There's not a single straight

 ;; line of force in the human body at all!  (A straight line of force

 ;; would correspond to some sort of jet or rocket propulseion.)

 

 (defn vector-motor-control

   "Create a function that accepts a sequence of Vector3f objects that

    describe the torque to be applied to each part of the body."

   [body]

   (let [nodes (node-seq body)

         controls (keep #(.getControl % RigidBodyControl) nodes)]

     (fn [torques]

       (map #(.applyTorque %1 %2)

            controls torques))))

 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

 #+end_src

  

 ## note -- might want to add a lower dimensional, discrete version of

 ## this if it proves useful from a x-modal clustering perspective.

 

 * Examples

 

 #+name: test-body

 #+begin_src clojure

 (ns cortex.test.body

   (:use (cortex world util body))

   (:import

    com.jme3.math.Vector3f

    com.jme3.math.ColorRGBA

    com.jme3.bullet.joints.Point2PointJoint

    com.jme3.bullet.control.RigidBodyControl

    com.jme3.system.NanoTimer))

 

 (defn worm-segments

   "Create multiple evenly spaced box segments. They're fabulous!"

   [segment-length num-segments interstitial-space radius]

   (letfn [(nth-segment

             [n]

             (box segment-length radius radius :mass 0.1

                  :position

                  (Vector3f.

                   (* 2 n (+ interstitial-space segment-length)) 0 0)

                  :name (str "worm-segment" n)

                  :color (ColorRGBA/randomColor)))]

     (map nth-segment (range num-segments))))

 

 (defn connect-at-midpoint

   "Connect two physics objects with a Point2Point joint constraint at

   the point equidistant from both objects' centers."

   [segmentA segmentB]

   (let [centerA (.getWorldTranslation segmentA)

         centerB (.getWorldTranslation segmentB)

         midpoint (.mult (.add centerA centerB) (float 0.5))

         pivotA (.subtract midpoint centerA)

         pivotB (.subtract midpoint centerB)

         

         ;; A side-effect of creating a joint registers

         ;; it with both physics objects which in turn

         ;; will register the joint with the physics system

         ;; when the simulation is started.

         joint (Point2PointJoint.

                (.getControl segmentA RigidBodyControl)

                (.getControl segmentB RigidBodyControl)

                pivotA

                pivotB)]

     segmentB))

 

 (defn eve-worm

   "Create a worm-like body bound by invisible joint constraints."

   []

   (let [segments (worm-segments 0.2 5 0.1 0.1)]

     (dorun (map (partial apply connect-at-midpoint)

                 (partition 2 1 segments)))

     (nodify "worm" segments)))

 

 (defn worm-pattern

   "This is a simple, mindless motor control pattern that drives the

    second segment of the worm's body at an offset angle with

    sinusoidally varying strength."

   [time]

   (let [angle (* Math/PI (/ 9 20))

         direction (Vector3f. 0 (Math/sin angle) (Math/cos angle))]

     [Vector3f/ZERO

      (.mult

       direction

       (float (* 2 (Math/sin (* Math/PI 2 (/ (rem time 300 ) 300))))))

      Vector3f/ZERO

      Vector3f/ZERO

      Vector3f/ZERO]))

 

 (defn test-motor-control

   "Testing motor-control:

    You should see a multi-segmented worm-like object fall onto the

    table and begin writhing and moving."

   []

   (let [worm (eve-worm)

         time (atom 0)

         worm-motor-map (vector-motor-control worm)]

     (world

      (nodify [worm

               (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0

                    :color ColorRGBA/Gray)])

      standard-debug-controls

      (fn [world]

        (enable-debug world)

        (light-up-everything world)

        (comment

          (com.aurellem.capture.Capture/captureVideo

           world

           (file-str "/home/r/proj/cortex/tmp/moving-worm")))

        )

      

      (fn [_ _]

        (swap! time inc)

        (Thread/sleep 20)

        (dorun (worm-motor-map

                (worm-pattern @time)))))))

 

 (defn test-proprioception

   "Testing proprioception:

    You should see two foating bars, and a printout of pitch, yaw, and

    roll. Pressing key-r/key-t should move the blue bar up and down and

    change only the value of pitch. key-f/key-g moves it side to side

    and changes yaw. key-v/key-b will spin the blue segment clockwise

    and counterclockwise, and only affect roll."

   []

   (let [hand    (box 1 0.2 0.2 :position (Vector3f. 0 2 0)

                      :mass 0 :color ColorRGBA/Green)

         finger (box 1 0.2 0.2 :position (Vector3f. 2.4 2 0)

                     :mass 1 :color (ColorRGBA. 0.20 0.40 0.99 1.0))

         floor   (box 10 0.5 10 :position (Vector3f. 0 -5 0)

                      :mass 0 :color ColorRGBA/Gray)

 

         move-up? (atom false)

         move-down? (atom false)

         move-left? (atom false)

         move-right? (atom false)

         roll-left? (atom false)

         roll-right? (atom false)

         control (.getControl finger RigidBodyControl)

         joint

         (doto 

             (Point2PointJoint.

              (.getControl hand RigidBodyControl)

              control

              (Vector3f. 1.2 0 0)

              (Vector3f. -1.2 0 0 ))

           (.setCollisionBetweenLinkedBodys false))

         time (atom 0)]

     (world

      (nodify [hand finger floor])

      (merge standard-debug-controls

             {"key-r" (fn [_ pressed?] (reset! move-up? pressed?))

              "key-t" (fn [_ pressed?] (reset! move-down? pressed?))

              "key-f" (fn [_ pressed?] (reset! move-left? pressed?))

              "key-g" (fn [_ pressed?] (reset! move-right? pressed?))

              "key-v" (fn [_ pressed?] (reset! roll-left? pressed?))

              "key-b" (fn [_ pressed?] (reset! roll-right? pressed?))})     

      (fn [world]

        (.setTimer world (NanoTimer.))

        (set-gravity world (Vector3f. 0 0 0))

        (.setMoveSpeed (.getFlyByCamera world) 50)

        (.setRotationSpeed (.getFlyByCamera world) 50)

        (light-up-everything world))

      (fn [_ _]

        (if @move-up?

          (.applyTorque control

                        (.mult (.getPhysicsRotation control)

                        (Vector3f. 0 0 10))))

        (if @move-down?

          (.applyTorque control

                        (.mult (.getPhysicsRotation control)

                               (Vector3f. 0 0 -10))))

        (if @move-left?

          (.applyTorque control

                        (.mult (.getPhysicsRotation control)

                               (Vector3f. 0 10 0))))

        (if @move-right?

          (.applyTorque control

                        (.mult (.getPhysicsRotation control)

                               (Vector3f. 0 -10 0))))

        (if @roll-left?

          (.applyTorque control

                        (.mult (.getPhysicsRotation control)

                               (Vector3f. -1 0 0))))

        (if @roll-right?

          (.applyTorque control

                        (.mult (.getPhysicsRotation control)

                               (Vector3f. 1 0 0))))

 

        (if (= 0 (rem (swap! time inc) 2000))

          (do

            (apply

             (comp

              println-repl

              #(format "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n" %1 %2 %3))

             (joint-proprioception joint))))))))

 #+end_src

 

 

 * COMMENT code-limbo

 #+begin_src clojure

 ;;(.loadModel 

 ;; (doto (asset-manager)

 ;;   (.registerLoader BlenderModelLoader (into-array String ["blend"])))

 ;;   "Models/person/person.blend")

 

 

 (defn load-blender-model

   "Load a .blend file using an asset folder relative path."

   [^String model]

   (.loadModel

    (doto (asset-manager)

      (.registerLoader BlenderModelLoader (into-array String ["blend"])))

    model))

 

 

 (defn view-model [^String model]

   (view 

    (.loadModel 

     (doto (asset-manager)

       (.registerLoader BlenderModelLoader (into-array String ["blend"])))

     model)))

 

 (defn load-blender-scene [^String model]

   (.loadModel

    (doto (asset-manager)

      (.registerLoader BlenderLoader (into-array String ["blend"])))

     model))

 

 (defn worm

   []

   (.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml"))

 

 (defn oto

   []

   (.loadModel (asset-manager) "Models/Oto/Oto.mesh.xml"))

 

 (defn sinbad

   []

   (.loadModel (asset-manager) "Models/Sinbad/Sinbad.mesh.xml"))

 

 (defn worm-blender

   []

   (first (seq (.getChildren (load-blender-model

                              "Models/anim2/simple-worm.blend")))))

 

 (defn body

   "given a node with a SkeletonControl, will produce a body sutiable

   for AI control with movement and proprioception."

   [node]

   (let [skeleton-control (.getControl node SkeletonControl)

         krc (KinematicRagdollControl.)]

     (comment

     (dorun

      (map #(.addBoneName krc %)

           ["mid2" "tail" "head" "mid1" "mid3" "mid4" "Dummy-Root" ""]

            ;;"mid2" "mid3" "tail" "head"]

         )))

     (.addControl node krc)

     (.setRagdollMode krc)

     )

   node

   )

 (defn show-skeleton [node]

   (let [sd

 

         (doto

             (SkeletonDebugger. "aurellem-skel-debug"

                                (skel node))

           (.setMaterial (green-x-ray)))]

     (.attachChild node sd)

     node))

 

 

   

 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

 

 ;; this could be a good way to give objects special properties like

 ;; being eyes and the like

 

 (.getUserData 

  (.getChild

   (load-blender-model "Models/property/test.blend") 0)

  "properties")

 

 ;; the properties are saved along with the blender file.

 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

 

 

 

 

 (defn init-debug-skel-node

   [f debug-node skeleton]

   (let [bones 

         (map #(.getBone skeleton %)

              (range (.getBoneCount skeleton)))]

   (dorun (map #(.setUserControl % true) bones)) 

   (dorun (map (fn [b]

                 (println (.getName b)

                          " -- " (f b)))

               bones))

   (dorun

    (map #(.attachChild

           debug-node 

           (doto 

               (sphere 0.1

                       :position (f %)

                       :physical? false)

           (.setMaterial (green-x-ray))))

         bones)))

     debug-node)

 

 (import jme3test.bullet.PhysicsTestHelper)

 

 

 (defn test-zzz [the-worm world value]

   (if (not value)

     (let [skeleton (skel the-worm)]

       (println-repl "enabling bones")

       (dorun

        (map

         #(.setUserControl (.getBone skeleton %) true)

         (range (.getBoneCount skeleton))))

 

 

       (let [b (.getBone skeleton 2)]

         (println-repl "moving " (.getName b))

         (println-repl (.getLocalPosition b))

         (.setUserTransforms b

                             Vector3f/UNIT_X

                             Quaternion/IDENTITY

                             ;;(doto (Quaternion.)

                              ;; (.fromAngles (/ Math/PI 2)

                              ;;              0

                              ;;              0

                                           

                             (Vector3f. 1 1 1))

         )

       

       (println-repl "hi! <3"))))

 

 

 (defn test-ragdoll []

   

   (let [the-worm

         

          ;;(.loadModel (asset-manager) "Models/anim2/Cube.mesh.xml")

          (doto (show-skeleton (worm-blender))

            (.setLocalTranslation (Vector3f. 0 10 0))

          ;;(worm)

          ;;(oto)

          ;;(sinbad)

         )

     ]

 

 

     (.start 

      (world

       (doto (Node.)

         (.attachChild the-worm))

       {"key-return" (fire-cannon-ball)

        "key-space" (partial test-zzz the-worm)

        }

       (fn [world]

         (light-up-everything world)

         (PhysicsTestHelper/createPhysicsTestWorld

          (.getRootNode world)

          (asset-manager)

          (.getPhysicsSpace

           (.getState (.getStateManager world) BulletAppState)))

         (set-gravity world Vector3f/ZERO)

         ;;(.setTimer world (NanoTimer.))

         ;;(org.lwjgl.input.Mouse/setGrabbed false)

         )

       no-op

       )

   

      

      )))

 

 

 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

 ;;; here is the ragdoll stuff

 

 (def worm-mesh (.getMesh (.getChild (worm-blender) 0)))

 (def mesh worm-mesh)

 

 (.getFloatBuffer mesh VertexBuffer$Type/Position)

 (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)

 (.getData  (.getBuffer mesh VertexBuffer$Type/BoneIndex))

 

 

 (defn position [index]

   (.get

    (.getFloatBuffer worm-mesh VertexBuffer$Type/Position)

      index))

 

 (defn bones [index]

   (.get

    (.getData  (.getBuffer mesh VertexBuffer$Type/BoneIndex))

    index))

 

 (defn bone-weights [index]

   (.get

    (.getFloatBuffer mesh VertexBuffer$Type/BoneWeight)

    index))

 

 

 

 (defn vertex-bones [vertex]

   (vec (map (comp int bones) (range (* vertex 4) (+ (* vertex 4) 4)))))

 

 (defn vertex-weights [vertex]

   (vec (map (comp float bone-weights) (range (* vertex 4) (+ (* vertex 4) 4)))))

 

 (defn vertex-position [index]

   (let [offset (* index 3)]

     (Vector3f. (position offset)

                (position (inc offset))

                (position (inc(inc offset))))))

 

 (def vertex-info (juxt vertex-position vertex-bones vertex-weights))

 

 (defn bone-control-color [index]

   (get {[1 0 0 0] ColorRGBA/Red

         [1 2 0 0] ColorRGBA/Magenta

         [2 0 0 0] ColorRGBA/Blue}

        (vertex-bones index)

        ColorRGBA/White))

 

 (defn influence-color [index bone-num]

   (get

    {(float 0)   ColorRGBA/Blue

     (float 0.5) ColorRGBA/Green

     (float 1)   ColorRGBA/Red}

      ;; find the weight of the desired bone

    ((zipmap (vertex-bones index)(vertex-weights index))

     bone-num)

    ColorRGBA/Blue))

          

 (def worm-vertices (set (map vertex-info (range 60))))

 

 

 (defn test-info []

   (let [points (Node.)]

     (dorun

      (map #(.attachChild points %)

           (map #(sphere 0.01

                         :position (vertex-position %)

                         :color (influence-color % 1)

                         :physical? false)

                (range 60))))

     (view points)))

   

 

 (defrecord JointControl [joint physics-space]

   PhysicsControl

   (setPhysicsSpace [this space]

     (dosync

      (ref-set (:physics-space this) space))

     (.addJoint space (:joint this)))

   (update [this tpf])

   (setSpatial [this spatial])

   (render [this rm vp])

   (getPhysicsSpace [this] (deref (:physics-space this)))

   (isEnabled [this] true)

   (setEnabled [this state]))

   

 (defn add-joint

   "Add a joint to a particular object. When the object is added to the

   PhysicsSpace of a simulation, the joint will also be added"

   [object joint]

   (let [control (JointControl. joint (ref nil))]

     (.addControl object control))

   object)

 

 

 (defn hinge-world

   []

   (let [sphere1 (sphere)

         sphere2 (sphere 1 :position (Vector3f. 3 3 3))

         joint (Point2PointJoint.

                (.getControl sphere1 RigidBodyControl)

                (.getControl sphere2 RigidBodyControl) 

                Vector3f/ZERO (Vector3f. 3 3 3))]

     (add-joint sphere1 joint)

     (doto (Node. "hinge-world")

       (.attachChild sphere1)

       (.attachChild sphere2))))

 

 

 (defn test-joint []

   (view (hinge-world)))

 

 ;; (defn copier-gen []

 ;;   (let [count (atom 0)]

 ;;     (fn [in]

 ;;       (swap! count inc)

 ;;       (clojure.contrib.duck-streams/copy

 ;;        in (File. (str "/home/r/tmp/mao-test/clojure-images/"

 ;;                      ;;/home/r/tmp/mao-test/clojure-images

 ;;                (format "%08d.png" @count)))))))

 ;; (defn decrease-framerate []

 ;;   (map

 ;;    (copier-gen)

 ;;    (sort

 ;;     (map first

 ;;          (partition

 ;;           4

 ;;           (filter #(re-matches #".*.png$" (.getCanonicalPath %))

 ;;                   (file-seq

 ;;                    (file-str

 ;;                     "/home/r/media/anime/mao-temp/images"))))))))

 

 

 

 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

 

 (defn proprioception

   "Create a proprioception map that reports the rotations of the

   various limbs of the creature's body"

   [creature]

   [#^Node creature]

   (let [

         nodes (node-seq creature)

         joints

         (map

          :joint 

          (filter

           #(isa? (class %) JointControl)

           (reduce

            concat

            (map (fn [node]

                   (map (fn [num] (.getControl node num))

                        (range (.getNumControls node))))

                 nodes))))]

     (fn []

       (reduce concat (map relative-positions (list (first joints)))))))

 

 

 (defn skel [node]

   (doto

       (.getSkeleton

        (.getControl node SkeletonControl))

     ;; this is necessary to force the skeleton to have accurate world

     ;; transforms before it is rendered to the screen. 

     (.resetAndUpdate)))

 

 (defn green-x-ray []

   (doto (Material. (asset-manager)

                    "Common/MatDefs/Misc/Unshaded.j3md")

     (.setColor "Color" ColorRGBA/Green)

     (-> (.getAdditionalRenderState)

         (.setDepthTest false))))

 

 (defn test-worm []

   (.start

    (world

     (doto (Node.)

       ;;(.attachChild (point-worm))

       (.attachChild (load-blender-model

                      "Models/anim2/joint-worm.blend"))

                       

       (.attachChild (box 10 1 10

                          :position (Vector3f. 0 -2 0) :mass 0

                          :color (ColorRGBA/Gray))))

     {

      "key-space" (fire-cannon-ball)

      }

     (fn [world]

       (enable-debug world)

       (light-up-everything world)

       ;;(.setTimer world (NanoTimer.))

       )

     no-op)))

 

 

 

 ;; defunct movement stuff

 (defn torque-controls [control]

   (let [torques

         (concat

          (map #(Vector3f. 0 (Math/sin %) (Math/cos %))

               (range 0 (* Math/PI 2) (/ (* Math/PI 2) 20)))

          [Vector3f/UNIT_X])]

     (map (fn [torque-axis]

            (fn [torque]

              (.applyTorque

               control

               (.mult (.mult (.getPhysicsRotation control)

                             torque-axis)

                      (float

                       (* (.getMass control) torque))))))

          torques)))

 

 (defn motor-map

   "Take a creature and generate a function that will enable fine

    grained control over all the creature's limbs."  

   [#^Node creature]

   (let [controls (keep #(.getControl % RigidBodyControl)

                        (node-seq creature))

         limb-controls (reduce concat (map torque-controls controls))

         body-control  (partial map #(%1 %2) limb-controls)]

     body-control))

 

 (defn test-motor-map

   "see how torque works."

   []

   (let [finger (box 3 0.5 0.5 :position (Vector3f. 0 2 0)

                     :mass 1 :color ColorRGBA/Green)

         motor-map (motor-map finger)]

     (world

      (nodify [finger

               (box 10 0.5 10 :position (Vector3f. 0 -5 0) :mass 0

                    :color ColorRGBA/Gray)])

      standard-debug-controls

      (fn [world]

        (set-gravity world Vector3f/ZERO)

        (light-up-everything world)

        (.setTimer world (NanoTimer.)))

      (fn [_ _]

        (dorun (motor-map [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0]))))))

 

 

 #+end_src

 

 

 

 

 

 

 

 * COMMENT generate Source.

 #+begin_src clojure :tangle ../src/cortex/body.clj

 <<proprioception>>

 <<motor-control>>

 #+end_src

 

 #+begin_src clojure :tangle ../src/cortex/test/body.clj

 <<test-body>>

 #+end_src