#+title: Building a 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

 

 

 * Design Constraints

 

 I use [[www.blender.org/][blender]] to design bodies.  The design of the bodies is

 determined by the requirements of the AI that will use them. The

 bodies must be easy for an AI to sense and control, and they must be

 relatively simple for jMonkeyEngine to compute.  

 

 ** Bag of Bones

 

 How to create such a body? One option I ultimately rejected is to use

 blender's [[http://wiki.blender.org/index.php/Doc:2.6/Manual/Rigging/Armatures][armature]] system. The idea would have been to define a mesh

 which describes the creature's entire body. To this you add an

 (skeleton) which deforms this mesh. This technique is used extensively

 to model humans and create realistic animations. It is hard to use for

 my purposes because it is difficult to update the creature's Physics

 Collision Mesh in tandem with its Geometric Mesh under the influence

 of the armature. Withouth this the creature will not be able to grab

 things in its environment, and it won't be able to tell where its

 physical body is by using its eyes. Also, armatures do not specify

 any rotational limits for a joint, making it hard to model elbows,

 shoulders, etc.

 

 ** EVE

 

 Instead of using the human-like "deformable bag of bones" approach, I

 decided to base my body plans on the robot EVE from the movie wall-E. 

 

 #+caption: EVE from the movie WALL-E.  This body plan turns out to be much better suited to my purposes than a more human-like one.

 [[../images/Eve.jpg]]

 

 EVE's body is composed of several rigid components that are held

 together by invisible joint constraints. This is what I mean by

 "eve-like". The main reason that I use eve-style bodies is so that

 there will be correspondence between the AI's vision and the physical

 presence of its body. Each individual section is simulated by a

 separate rigid body that corresponds exactly with its visual

 representation and does not change. Sections are connected by

 invisible joints that are well supported in jMonkyeEngine. Bullet, the

 physics backend for jMonkeyEngine, can efficiently simulate hundreds

 of rigid bodies connected by joints. Sections do not have to stay as

 one piece forever; they can be dynamically replaced with multiple

 sections to simulate splitting in two. This could be used to simulate

 retractable claws or EVE's hands, which could coalece into one object

 in the movie.

 

 * Solidifying the Body

 

 Here is a hand designed eve-style in blender.

 

 #+attr_html: width="755"

 [[../images/hand-screenshot0.png]]

 

 If we load it directly into jMonkeyEngine, we get this:

 

 #+name: test-0

 #+begin_src clojure

 (ns cortex.test.body

   (:use (cortex world util body))

   (:import (com.aurellem.capture Capture RatchetTimer)

            (com.jme3.math Quaternion Vector3f)

            java.io.File))

 

 (def hand-path "Models/test-creature/hand.blend")

 

 (defn hand [] (load-blender-model hand-path))

 

 (defn setup [world]

   (let [cam (.getCamera world)]

     (println-repl cam)

     (.setLocation

      cam (Vector3f. 

           -6.9015837, 8.644911, 5.6043186))

     (.setRotation

      cam

      (Quaternion.

       0.14046453, 0.85894054, -0.34301838, 0.3533118)))

   (light-up-everything world)

   (.setTimer world (RatchetTimer. 60))

   world)

 

 (defn test-one []

   (world (hand)

          standard-debug-controls

          (comp

           #(Capture/captureVideo

             % (File. "/home/r/proj/cortex/render/body/1"))

           setup)

          no-op))

 #+end_src

 

 

 #+begin_src clojure :results silent

 (.start (cortex.test.body/test-one))

 #+end_src

 

 #+begin_html

 <div class="figure">

 <center>

 <video controls="controls" width="640">

   <source src="../video/ghost-hand.ogg" type="video/ogg"

 	  preload="none" poster="../images/aurellem-1280x480.png" />

 </video>

 </center>

 <p>The hand model directly loaded from blender. It has no physical

   presense in the simulation. </p>

 </div>

 #+end_html

 

 You will notice that the hand has no physical presence -- it's a

 hologram through which everything passes.  Therefore, the first thing

 to do is to make it solid.  Blender has physics simulation on par with

 jMonkeyEngine (they both use bullet as their physics backend), but it

 can be difficult to translate between the two systems, so for now I

 specify the mass of each object in blender and construct the physics

 shape based on the mesh in jMonkeyEngine.

 

 #+name: body-1

 #+begin_src clojure

 (defn physical!

   "Iterate through the nodes in creature and make them real physical

    objects in the simulation."

   [#^Node creature]

   (dorun

    (map

     (fn [geom]

       (let [physics-control

             (RigidBodyControl.

              (HullCollisionShape.

               (.getMesh geom))

              (if-let [mass (meta-data geom "mass")]

                (do

                  (println-repl

                   "setting" (.getName geom) "mass to" (float mass))

                  (float mass))

                (float 1)))]

         (.addControl geom physics-control)))

     (filter #(isa? (class %) Geometry )

             (node-seq creature)))))

 #+end_src

 

 =(physical!)= iterates through a creature's node structure, creating

 CollisionShapes for each geometry with the mass specified in that

 geometry's meta-data.

 

 #+name: test-1

 #+begin_src clojure 

 (in-ns 'cortex.test.body)

 

 (def normal-gravity 

   {"key-g" (fn [world _]

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

 

 (defn floor [] 

   (box 10 3 10 :position (Vector3f. 0 -10 0)

        :color ColorRGBA/Gray :mass 0))

 

 (defn test-two []

   (world (nodify

           [(doto (hand)

              (physical!))

           (floor)])

          (merge standard-debug-controls normal-gravity)

          (comp

           #(Capture/captureVideo

             % (File. "/home/r/proj/cortex/render/body/2"))

           #(do (set-gravity % Vector3f/ZERO) %)

           setup)

          no-op))

 #+end_src

 

 #+begin_html

 <div class="figure">

 <center>

 <video controls="controls" width="640">

   <source src="../video/crumbly-hand.ogg" type="video/ogg"

 	  preload="none" poster="../images/aurellem-1280x480.png" />

 </video>

 </center>

 <p>The hand now has a physical presence, but there is nothing to hold

 it together.</p>

 </div>

 #+end_html

 

 Now that's some progress.

 

 

 * Joints

 

 Obviously, an AI is not going to be doing much just lying in pieces on

 the floor.  So, the next step to making a proper body is to connect

 those pieces together with joints.  jMonkeyEngine has a large array of

 joints available via bullet, such as Point2Point, Cone, Hinge, and a

 generic Six Degree of Freedom joint, with or without spring

 restitution.

 

 Although it should be possible to specify the joints using blender's

 physics system, and then automatically import them with jMonkeyEngine,

 the support isn't there yet, and there are a few problems with bullet

 itself that need to be solved before it can happen. 

 

 So, I will use the same system for specifying joints as I will do for

 some senses.  Each joint is specified by an empty node whose parent

 has the name "joints". Their orientation and meta-data determine what

 joint is created.

 

 #+attr_html: width="755"

 #+caption: joints hack in blender. Each empty node here will be transformed into a joint in jMonkeyEngine

 [[../images/hand-screenshot1.png]]

 

 The empty node in the upper right, highlighted in yellow, is the

 parent node of all the emptys which represent joints. The following

 functions must do three things to translate these into real joints:

 

  - Find the children of the "joints" node.

  - Determine the two spatials the joint it meant to connect.

  - Create the joint based on the meta-data of the empty node.

 

 ** Finding the Joints 

 #+name: joints-2 

 #+begin_src clojure

 (defvar 

   ^{:arglists '([creature])}

   joints

   (sense-nodes "joints")

   "Return the children of the creature's \"joints\" node.")

 #+end_src

 

 The higher order function =(sense-nodes)= from cortex.sense makes our

 first task very easy.

 

 ** Joint Targets and Orientation

 

 This technique for finding a joint's targets is very similiar to

 =(cortex.sense/closest-node)=.  A small cube, centered around the

 empty-node, grows exponentially until it intersects two /physical/

 objects. The objects are ordered according to the joint's rotation,

 with the first one being the object that has more negative coordinates

 in the joint's reference frame. Since the objects must be physical,

 the empty-node itself escapes detection. Because the objects must be

 physical, =(joint-targets)= must be called /after/ =(physical!)= is

 called.

 

 #+name: joints-3

 #+begin_src clojure 

 (defn joint-targets

   "Return the two closest two objects to the joint object, ordered

   from bottom to top according to the joint's rotation."

   [#^Node parts #^Node joint]

   (loop [radius (float 0.01)]

     (let [results (CollisionResults.)]

       (.collideWith

        parts

        (BoundingBox. (.getWorldTranslation joint)

                      radius radius radius)

        results)

       (let [targets

             (distinct

              (map  #(.getGeometry %) results))]

         (if (>= (count targets) 2)

           (sort-by

            #(let [v

                   (jme-to-blender

                    (.mult

                     (.inverse (.getWorldRotation joint))

                     (.subtract (.getWorldTranslation %)

                                (.getWorldTranslation joint))))]

               (println-repl (.getName %) ":" v)

               (.dot (Vector3f. 1 1 1)

                     v))                  

            (take 2 targets))

           (recur (float (* radius 2))))))))

 #+end_src

 

 ** Generating Joints

 

 This long chunk of code iterates through all the different ways of

 specifying joints using blender meta-data and converts each one to the

 appropriate jMonkyeEngine joint.

 

 #+name: joints-4

 #+begin_src clojure

 (defmulti joint-dispatch

   "Translate blender pseudo-joints into real JME joints."

   (fn [constraints & _] 

     (:type constraints)))

 

 (defmethod joint-dispatch :point

   [constraints control-a control-b pivot-a pivot-b rotation]

   (println-repl "creating POINT2POINT joint")

   ;; bullet's point2point joints are BROKEN, so we must use the

   ;; generic 6DOF joint instead of an actual Point2Point joint!

 

   ;; should be able to do this:

   (comment 

     (Point2PointJoint.

      control-a

      control-b

      pivot-a

      pivot-b))

 

   ;; but instead we must do this:

   (println-repl "substuting 6DOF joint for POINT2POINT joint!")

   (doto

       (SixDofJoint.

        control-a

        control-b

        pivot-a

        pivot-b

        false)

     (.setLinearLowerLimit Vector3f/ZERO)

     (.setLinearUpperLimit Vector3f/ZERO)))

 

 (defmethod joint-dispatch :hinge

   [constraints control-a control-b pivot-a pivot-b rotation]

   (println-repl "creating HINGE joint")

   (let [axis

         (if-let

             [axis (:axis constraints)]

           axis

           Vector3f/UNIT_X)

         [limit-1 limit-2] (:limit constraints)

         hinge-axis

         (.mult

          rotation

          (blender-to-jme axis))]

     (doto

         (HingeJoint.

          control-a

          control-b

          pivot-a

          pivot-b

          hinge-axis

          hinge-axis)

       (.setLimit limit-1 limit-2))))

 

 (defmethod joint-dispatch :cone

   [constraints control-a control-b pivot-a pivot-b rotation]

   (let [limit-xz (:limit-xz constraints)

         limit-xy (:limit-xy constraints)

         twist    (:twist constraints)]

     

     (println-repl "creating CONE joint")

     (println-repl rotation)

     (println-repl

      "UNIT_X --> " (.mult rotation (Vector3f. 1 0 0)))

     (println-repl

      "UNIT_Y --> " (.mult rotation (Vector3f. 0 1 0)))

     (println-repl

      "UNIT_Z --> " (.mult rotation (Vector3f. 0 0 1)))

     (doto

         (ConeJoint.

          control-a

          control-b

          pivot-a

          pivot-b

          rotation

          rotation)

       (.setLimit (float limit-xz)

                  (float limit-xy)

                  (float twist)))))

 

 (defn connect

   "Create a joint between 'obj-a and 'obj-b at the location of

   'joint. The type of joint is determined by the metadata on 'joint.

 

    Here are some examples:

    {:type :point}

    {:type :hinge  :limit [0 (/ Math/PI 2)] :axis (Vector3f. 0 1 0)}

    (:axis defaults to (Vector3f. 1 0 0) if not provided for hinge joints)

 

    {:type :cone :limit-xz 0]

                 :limit-xy 0]

                 :twist 0]}   (use XZY rotation mode in blender!)"

   [#^Node obj-a #^Node obj-b #^Node joint]

   (let [control-a (.getControl obj-a RigidBodyControl)

         control-b (.getControl obj-b RigidBodyControl)

         joint-center (.getWorldTranslation joint)

         joint-rotation (.toRotationMatrix (.getWorldRotation joint))

         pivot-a (world-to-local obj-a joint-center)

         pivot-b (world-to-local obj-b joint-center)]

    

     (if-let [constraints

              (map-vals

               eval

               (read-string

                (meta-data joint "joint")))]

       ;; 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.

       (do

         (println-repl "creating joint between"

                       (.getName obj-a) "and" (.getName obj-b))

         (joint-dispatch constraints

                         control-a control-b

                         pivot-a pivot-b

                         joint-rotation))

       (println-repl "could not find joint meta-data!"))))

 #+end_src

 

 Creating joints is now a matter applying =(connect)= to each joint

 node.

 

 #+begin_src clojure

 (defn joints!

   "Connect the solid parts of the creature with physical joints. The

    joints are taken from the \"joints\" node in the creature."

   [#^Node creature]

   (dorun

    (map

     (fn [joint]

       (let [[obj-a obj-b] (joint-targets creature joint)]

         (connect obj-a obj-b joint)))

     (joints creature))))

 #+end_src

 

 

 ** Round 3

 

 Now we can test the hand in all its glory.

 

 #+begin_src clojure 

 (in-ns 'cortex.test.body)

 

 (def debug-control 

   {"key-h" (fn [world val]

              (if val (enable-debug world)))

 

    "key-u" (fn [world _] (set-gravity world Vector3f/ZERO))

    })

   

 (defn test-three []

   (world (nodify

           [(doto (hand)

            (physical!)

            (joints!) )

            (floor)])

          (merge standard-debug-controls debug-control

                 normal-gravity)

          (comp

           #(Capture/captureVideo

             % (File. "/home/r/proj/cortex/render/body/3"))

           #(do (set-gravity % Vector3f/ZERO) %)

           setup)

          no-op))

 #+end_src

 

 =(physical!)= makes the hand solid, then =(joints!)= connects each

 piece together. 

 

 

 #+begin_html

 <div class="figure">

 <center>

 <video controls="controls" width="640">

   <source src="../video/full-hand.ogg" type="video/ogg"

 	  preload="none" poster="../images/aurellem-1280x480.png" />

 </video>

 </center>

 <p>Now the hand is physical and has joints.</p>

 </div>

 #+end_html

 

 The joints are visualized as green connections between each segment

 for debug purposes. You can see that they correspond to the empty

 nodes in the blender file.

 

 * Wrap-Up!

 

 It is convienent to combine =(physical!)= and =(joints!)= into one

 function that completely creates the creature's physical body.

 

 #+name: joints-4

 #+begin_src clojure

 (defn body!

   "Endow the creature with a physical body connected with joints.  The

    particulars of the joints and the masses of each pody part are

    determined in blender."

   [#^Node creature]

   (physical! creature)

   (joints! creature))

 #+end_src

 

 * Bookkeeping

 

 Header; here for completeness.

 

 #+name: body-0

 #+begin_src clojure

 (ns cortex.body

   "Assemble a physical creature using the definitions found in a

    specially prepared blender file. Creates rigid bodies and joints so

    that a creature can have a physical presense in the simulation."

   {:author "Robert McIntyre"}

   (:use (cortex world util sense))

   (:use clojure.contrib.def)

   (:import

    (com.jme3.math Vector3f Quaternion Vector2f Matrix3f)

    (com.jme3.bullet.joints

     SixDofJoint Point2PointJoint HingeJoint ConeJoint)

    com.jme3.bullet.control.RigidBodyControl

    com.jme3.collision.CollisionResults

    com.jme3.bounding.BoundingBox

    com.jme3.scene.Node

    com.jme3.scene.Geometry

    com.jme3.bullet.collision.shapes.HullCollisionShape))

 #+end_src

 

 * Source 

 

 Dylan -- I'll fill these in later

 - cortex.body

 - cortex.test.body

 - blender files

 

 * COMMENT Examples

 

 #+name: test-body

 #+begin_src clojure

 (ns cortex.test.body

   (:use (cortex world util body))

   (:require cortex.silly)

   (:import

    com.jme3.math.Vector3f

    com.jme3.math.ColorRGBA

    com.jme3.bullet.joints.Point2PointJoint

    com.jme3.bullet.control.RigidBodyControl

    com.jme3.system.NanoTimer

    com.jme3.math.Quaternion))

 

 (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 join-at-point [obj-a obj-b world-pivot]

   (cortex.silly/joint-dispatch

    {:type :point}

    (.getControl obj-a RigidBodyControl)

    (.getControl obj-b RigidBodyControl)

    (cortex.silly/world-to-local obj-a world-pivot)

    (cortex.silly/world-to-local obj-b world-pivot)

    nil

    ))

 

 (import com.jme3.bullet.collision.PhysicsCollisionObject)

 

 (defn blab-* []

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

                      :mass 0 :color ColorRGBA/Green)

         finger (box 0.5 0.2 0.2 :position (Vector3f. 2.4 0 0)

                     :mass 1 :color ColorRGBA/Red)

         connection-point (Vector3f. 1.2 0 0)

         root (nodify [hand finger])]

 

     (join-at-point hand finger (Vector3f. 1.2 0 0))

     

     (.setCollisionGroup

      (.getControl hand RigidBodyControl)

      PhysicsCollisionObject/COLLISION_GROUP_NONE)

     (world

      root

      standard-debug-controls

      (fn [world]

        (enable-debug world)

        (.setTimer world (com.aurellem.capture.RatchetTimer. 60))

        (set-gravity world Vector3f/ZERO)

        )

      no-op)))

 (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

     (comp 

      println-repl

      (fn [[p y r]]

        (format

         "pitch: %1.2f\nyaw: %1.2f\nroll: %1.2f\n"

         p y r)))

     prop-data)))

 

 

 

 

 (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 0.2 1 0.2 :position (Vector3f. 0 0 0)

                      :mass 0 :color ColorRGBA/Green :name "hand")

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

                     :mass 1 :color ColorRGBA/Red :name "finger")

         joint-node (box 0.1 0.05 0.05 :color ColorRGBA/Yellow

                         :position (Vector3f. 0 1.2 0)

                         :rotation (doto (Quaternion.)

                                     (.fromAngleAxis

                                      (/ Math/PI 2)

                                      (Vector3f. 0 0 1)))

                         :physical? false)

         joint (join-at-point hand finger (Vector3f. 0 1.2 0 ))

         creature (nodify [hand finger joint-node])

         finger-control (.getControl finger RigidBodyControl)

         hand-control (.getControl hand RigidBodyControl)]

     

 

     (let

         ;; *******************************************

                 

         [floor   (box 10 10 10 :position (Vector3f. 0 -15 0)

                      :mass 0 :color ColorRGBA/Gray)

         

         root (nodify [creature floor])

         prop (joint-proprioception creature joint-node)

         prop-view (proprioception-debug-window)

         

         controls

         (merge standard-debug-controls

                {"key-o"

                 (fn [_ _] (.setEnabled finger-control true))

                 "key-p"

                 (fn [_ _] (.setEnabled finger-control false))

                 "key-k"

                 (fn [_ _] (.setEnabled hand-control true))

                 "key-l"

                 (fn [_ _] (.setEnabled hand-control false))

                 "key-i"

                 (fn [world _] (set-gravity world (Vector3f. 0 0 0)))

                 "key-period"

                 (fn [world _]

                   (.setEnabled finger-control false)

                   (.setEnabled hand-control false)

                   (.rotate creature (doto (Quaternion.)

                                       (.fromAngleAxis

                                        (float (/ Math/PI 15))

                                        (Vector3f. 0 0 -1))))

                                               

                   (.setEnabled finger-control true)

                   (.setEnabled hand-control true)

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

                   )

                 

                   

                 }

                )

 

         ]

     (comment

       (.setCollisionGroup

        (.getControl hand RigidBodyControl)

        PhysicsCollisionObject/COLLISION_GROUP_NONE)

       )

     (apply

      world

      (with-movement

        hand

        ["key-y" "key-u" "key-h" "key-j" "key-n" "key-m"]

        [10 10 10 10 1 1]

        (with-movement

          finger

          ["key-r" "key-t" "key-f" "key-g" "key-v" "key-b"]

          [1 1 10 10 10 10]

          [root

           controls

           (fn [world]

             (.setTimer world (com.aurellem.capture.RatchetTimer. 60))

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

             (light-up-everything world))

           (fn [_ _] (prop-view (list (prop))))]))))))

 

 #+end_src

 

 #+results: test-body

 : #'cortex.test.body/test-proprioception

 

 

 * 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]))))))

 

 (defn joint-proprioception [#^Node parts #^Node joint]

   (let [[obj-a obj-b] (joint-targets parts joint)

         joint-rot (.getWorldRotation joint)

         pre-inv-a (.inverse (.getWorldRotation obj-a))

         x (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_X))

         y (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Y))

         z (.mult pre-inv-a (.mult joint-rot Vector3f/UNIT_Z))

 

         x Vector3f/UNIT_Y

         y Vector3f/UNIT_Z

         z Vector3f/UNIT_X

 

 

         tmp-rot-a (.getWorldRotation obj-a)]

     (println-repl "x:" (.mult tmp-rot-a x))

     (println-repl "y:" (.mult tmp-rot-a y))

     (println-repl "z:" (.mult tmp-rot-a z))

     (println-repl "rot-a" (.getWorldRotation obj-a))

     (println-repl "rot-b" (.getWorldRotation obj-b))

     (println-repl "joint-rot" joint-rot)

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

             ;;inv-a (.inverse rot-a)

             rot-b (.getWorldRotation obj-b)

             ;;relative (.mult rot-b inv-a)

             basis (doto (Matrix3f.)

                     (.setColumn 0 (.mult rot-a x))

                     (.setColumn 1 (.mult rot-a y))

                     (.setColumn 2 (.mult rot-a z)))

             rotation-about-joint

             (doto (Quaternion.)

               (.fromRotationMatrix 

                (.mult (.invert basis)

                       (.toRotationMatrix rot-b))))

             [yaw roll pitch]

             (seq (.toAngles rotation-about-joint nil))]

         ;;return euler angles of the quaternion around the new basis            

         [yaw roll pitch]))))

 

 #+end_src

 

 

 

 

 

 

 

 * COMMENT generate Source

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

 <<joints>>

 #+end_src

 

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

 <<test-0>>

 #+end_src