#+title: First attempt at a creature!

 #+author: Robert McIntyre

 #+email: rlm@mit.edu

 #+description: 

 #+keywords: simulation, jMonkeyEngine3, clojure

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

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

 

 * Intro 

 So far, I've made the following senses -- 

  - Vision

  - Hearing

  - Touch

  - Proprioception

 

 And one effector: 

  - Movement

 

 However, the code so far has only enabled these senses, but has not

 actually implemented them.  For example, there is still a lot of work

 to be done for vision. I need to be able to create an /eyeball/ in

 simulation that can be moved around and see the world from different

 angles. I also need to determine weather to use log-polar or cartesian

 for the visual input, and I need to determine how/wether to

 disceritise the visual input.

 

 I also want to be able to visualize both the sensors and the

 effectors in pretty pictures. This semi-retarted creature will by my

 first attempt at bringing everything together.

 

 * The creature's body

 

 Still going to do an eve-like body in blender, but due to problems

 importing the joints, etc into jMonkeyEngine3, I',m going to do all

 the connecting here in clojure code, using the names of the individual

 components and trial and error. Later, I'll maybe make some sort of

 creature-building modifications to blender that support whatever

 discreitized senses I'm going to make.

 

 #+name: body-1

 #+begin_src clojure 

 (ns cortex.silly

   "let's play!"

   {:author "Robert McIntyre"})

 

 ;; TODO remove this!

 (require 'cortex.import)

 (cortex.import/mega-import-jme3)

 (use '(cortex world util body hearing touch vision))

 

 (rlm.rlm-commands/help)

 

 (declare joint-create get-subjective-position)

 

 (defn load-bullet []

   (let [sim (world (Node.) {} no-op no-op)]

     (.enqueue

      sim

      (fn []

        (.stop sim)))

     (.start sim)))

 

 (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 meta-data [blender-node key]

   (if-let [data (.getUserData blender-node "properties")]

     (.findValue data key)

     nil))

 

 (defn blender-to-jme

   "Convert from Blender coordinates to JME coordinates"

   [#^Vector3f in]

   (Vector3f. (.getX in)

              (.getZ in)

              (- (.getY in))))

 

 (defn jme-to-blender

   "Convert from JME coordinates to Blender coordinates"

   [#^Vector3f in]

   (Vector3f. (.getX in)

              (- (.getZ in))

              (.getY in)))

 

 (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]

   ;;(println (meta-data joint "joint"))

   (.getWorldRotation 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))))))))

 

 (defn connect

   "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 [center-a (.getWorldTranslation obj-a)

         center-b (.getWorldTranslation obj-b)

         joint-center (.getWorldTranslation joint)

         pivot-a (.subtract joint-center center-a)

         pivot-b (.subtract joint-center center-b)

         control-a (.getControl obj-a RigidBodyControl)

         control-b (.getControl obj-b RigidBodyControl)]

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

     (if-let [constraints

              (map-vals

               eval

               (read-string

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

 

       (let [joint-type (:type constraints)]

         (println-repl "creating joint between"

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

         (cond (= :point joint-type)

               (do

                 (println-repl "creating POINT joint")

                 (Point2PointJoint.

                  control-a

                  control-b

                  pivot-a

                  pivot-b))

               (= :hinge joint-type)

               (do

                 (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

                        (.getWorldRotation joint)

                        (blender-to-jme axis))]

                   (doto

                       (HingeJoint.

                        control-a

                        control-b

                        pivot-a

                        pivot-b

                        hinge-axis

                        hinge-axis)

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

               (= :cone joint-type)

               (do

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

                       limit-xy (:limit-xy constraints)

                       twist    (:twist constraints)]

                   

                   (println-repl "creating CONE joint")

                   (let [frame-a

                         (.toRotationMatrix

                          (doto (Quaternion.)

                            (.fromAngleAxis

                             (float

                              (.angleBetween

                               (.normalize pivot-a) Vector3f/UNIT_X))

                             (.normalize

                              (.cross pivot-a

                                      Vector3f/UNIT_X)))))

                          ]

                     (println-repl "pivot-a" pivot-a)

                     (println-repl

                      "angle between pivot-a and UNIT_X is"

                      (.angleBetween Vector3f/UNIT_X (.normalize pivot-a)))

                     (println-repl "frame-a:" frame-a)

                     (println-repl

                      "frame-a moves Vector3f/UNIT_X to"

                      (.mult frame-a Vector3f/UNIT_X ))

                     

                     

                     (doto

                         (ConeJoint.

                          control-a

                          control-b

                          pivot-a

                          pivot-b

 

                         

                          ;; frame-in-A

                          ;;frame-a

                          ;;frame-a

                          

                          (.toRotationMatrix

                           (doto (Quaternion.)

                             (.fromAngles

                              0 0 (* -0.5 (/ Math/PI 2)))))

                          

                          

                          ;; frame-in-B

                          (.toRotationMatrix

                           (doto (Quaternion.)

                             (.fromAngles

                              0 0 (* -0.5 (/ Math/PI 2)))))

                          

 

                          )

                       (.setLimit (float limit-xz)

                                  (float limit-xy)

                                  (float twist))))))

               true

               (println-repl

                "joint-type" joint-type "not recognized")))

       

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

 

 

 (defn assemble-creature [#^Node pieces joints]

   (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 pieces))))

 

   (dorun

    (map

     (fn [joint]

       (let [[obj-a obj-b]

             (joint-targets pieces joint)]

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

     joints))

   pieces)

 

 (defn blender-creature [blender-path]

   (let [model (load-blender-model blender-path)

         joints

         (if-let [joint-node (.getChild model "joints")]

           (seq (.getChildren joint-node))

           (do (println-repl "could not find joints node")

               []))]

   (assemble-creature model joints)))

 

 (def hand "Models/creature1/one.blend")

 

 (def worm "Models/creature1/try-again.blend")

 

 (defn x-ray [#^ColorRGBA color]

   (doto (Material. (asset-manager)

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

     (.setColor "Color" color)

     (-> (.getAdditionalRenderState)

         (.setDepthTest false))))

 

 (defn test-creature [thing]

   (let [x-axis

         (box 1 0.01 0.01 :physical? false :color ColorRGBA/Red)

         y-axis

         (box 0.01 1 0.01 :physical? false :color ColorRGBA/Green)

         z-axis

         (box 0.01 0.01 1 :physical? false :color ColorRGBA/Blue)]

   (world

    (nodify [(blender-creature thing)

             (box 10 2 10 :position (Vector3f. 0 -9 0)

                  :color ColorRGBA/Gray :mass 0)

             x-axis y-axis z-axis

             ])

    standard-debug-controls

    (comp light-up-everything enable-debug

          (fn [world]

            (.setTimer world (NanoTimer.))

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

            (speed-up world)

            world

            ))

    no-op)))

 

 (defn world-setup [joint]

   (let [

 

         joint-position (Vector3f. 0 0 0)

         joint-rotation

         (.toRotationMatrix

          (.mult

           (doto (Quaternion.)

             (.fromAngleAxis

              (* 1 (/ Math/PI 4))

              (Vector3f. 0 0 1)))

           (.mult

            (doto (Quaternion.)

              (.fromAngleAxis

               (* 2 (/ Math/PI 4))

               (Vector3f. 0 1 0)))

            (doto (Quaternion.)

              (.fromAngleAxis

               (* 1 (/ Math/PI 2))

               (Vector3f. 0 0 1))))))

         top-position (.mult joint-rotation (Vector3f. 8 0 0))

         

         origin (doto

                    (sphere 0.1 :physical? false :color ColorRGBA/Cyan

                            :position top-position))

         top (doto

                 (sphere 0.1 :physical? false :color ColorRGBA/Yellow

                         :position top-position)

 

               (.addControl

                (RigidBodyControl.

                 (CapsuleCollisionShape. 0.5 1.5 1) (float 20))))

         bottom (doto

                    (sphere 0.1 :physical? false :color ColorRGBA/DarkGray

                            :position (Vector3f. 0 0 0))

                  (.addControl

                (RigidBodyControl.

                 (CapsuleCollisionShape. 0.5 1.5 1) (float 0))))

         table (box 10 2 10 :position (Vector3f. 0 -20 0)

                    :color ColorRGBA/Gray :mass 0)

         a (.getControl top RigidBodyControl)

         b (.getControl bottom RigidBodyControl)]

 

     (cond

      (= joint :cone)

      

        (doto (ConeJoint.

               a b

               (get-subjective-position joint-position top)

               (get-subjective-position joint-position bottom)

               joint-rotation

               joint-rotation

               )

 

               

          (.setLimit (* (/ 10) Math/PI)

                     (* (/ 4)  Math/PI)

                     0)))

   [origin top bottom table]))

             

 

 

 (defn test-joint [joint]

   (let [[origin top bottom floor] (world-setup joint)

         control (.getControl top RigidBodyControl)

         move-up? (atom false)

         move-down? (atom false)

         move-left? (atom false)

         move-right? (atom false)

         roll-left? (atom false)

         roll-right? (atom false)

         timer (atom 0)]

 

     (world

      (nodify [top bottom floor origin])

      (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]

        (light-up-everything world)

        (enable-debug world)

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

        )

      

      (fn [world _]

        (if (zero? (rem (swap! timer inc) 100))

          (do

          ;;  (println-repl @timer)

            (.attachChild (.getRootNode world)

                        (sphere 0.05 :color ColorRGBA/Yellow

                                :position (.getWorldTranslation top)

                                :physical? false))

            (.attachChild (.getRootNode world)

                          (sphere 0.05 :color ColorRGBA/LightGray

                                  :position (.getWorldTranslation bottom)

                                  :physical? false))))

 

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

 

 

 

 

 

 

 

 

 

 

 

 

 

 ;; please validate these.

 

 (defn get-subjective-position

    "Convert the world coordinates into coordinates relative to

    object (i.e. local coordinates), taking into account the rotation

    of object."

    [#^Vector3f world-coordinates object]

    ;; I don't know if it's important to take into account the rotation

    ;; of the object. If it isn't, then remove the multiplication-by-inverse.

    (.mult (.inverse (.getWorldRotation object))

           (.subtract world-coordinates (.getWorldTranslation object))))

    

 

 (defn get-subjective-rotation

   "cf get-subjective-position. Converts a rotation specified relative

 to the world's axes into a rotation specified in terms of the object's

   coordinate system."

   [#^Quaternion world-rotation object]

   (.mult (.inverse (.getWorldRotation object)) world-rotation))

 

 

 

 

 (defn joint-create "Connect objects 1 and 2 using a joint constraint. If

   only position is specified, creates a point-to-point joint at the

   given location

   in world coordinates. etc. etc. for other joints.

 To ensure consistency, I may alter this function

   so that it moves obj-1 to be at the apex of the cone.

 

   NOTE:

   In the usual construction method, you create a joint and, if your

   contraints are consistent, all the objects snap into position and

   orientation around it, otherwise the systen explodes.

 

   This construction method assumes that you have in mind a position and

   rotation for the joint, and that you have already put your objects

   at the required distances from that joint so that no snapping needs

   to occur. The radial distances ('pivot lengths') are therefore just set to be

   the pre-existing distances between the objects and the joint." 

   [#^Node obj-1

    #^Node obj-2

    #^Vector3f joint-position

    #^Quaternion joint-rotation

    span-1

    span-2

    twist

    ]

 

   (let

       [body-1 (.getControl obj-1 RigidBodyControl)

        body-2 (.getControl obj-2 RigidBodyControl)

        ]

     (doto (ConeJoint.

            body-1

            body-2

            (get-subjective-position joint-position body-1)

            (get-subjective-position joint-position body-2)

            ;; ALIGN X-AXIS OF OBJECT-1 WITH THE CENTRAL AXIS OF THE CONE TO

            ;;LOWER THE RISK OF INCONSISTENCY

            ;;(Matrix3f/IDENTITY)

            (.toRotationMatrix (get-subjective-rotation joint-rotation body-1))

            (.toRotationMatrix (get-subjective-rotation joint-rotation body-2))

            )

       (.setLimit

        span-1

        span-2

        twist))))

 

 

 

 

 

 

 #+end_src

 

 

 * COMMENT purgatory

 #+begin_src clojure

 (defn bullet-trans []

   (let [obj-a (sphere 0.5 :color ColorRGBA/Red

                       :position (Vector3f. -10 5 0))

         obj-b (sphere 0.5 :color ColorRGBA/Blue

                       :position (Vector3f. -10 -5 0)

                       :mass 0)

         control-a (.getControl obj-a RigidBodyControl)

         control-b (.getControl obj-b RigidBodyControl)

         swivel

         (.toRotationMatrix

          (doto (Quaternion.)

            (.fromAngleAxis (/ Math/PI 2)

                            Vector3f/UNIT_X)))]

     (doto 

         (ConeJoint.

          control-a control-b

          (Vector3f. 0 5 0)

          (Vector3f. 0 -5 0)

          swivel swivel)

       (.setLimit (* 0.6 (/ Math/PI 4))

                  (/ Math/PI 4)

                  (* Math/PI 0.8)))

     (world (nodify

             [obj-a obj-b])

             standard-debug-controls

             enable-debug

             no-op)))

 

 

 (defn bullet-trans* []

   (let [obj-a (box 1.5 0.5 0.5 :color ColorRGBA/Red

                    :position (Vector3f. 5 0 0)

                    :mass 90)

         obj-b (sphere 0.5 :color ColorRGBA/Blue

                       :position (Vector3f. -5 0 0)

                       :mass 0)

         control-a (.getControl obj-a RigidBodyControl)

         control-b (.getControl obj-b RigidBodyControl)

         move-up? (atom nil)

         move-down? (atom nil)

         move-left? (atom nil)

         move-right? (atom nil)

         roll-left? (atom nil)

         roll-right? (atom nil)

         force 100

         swivel

         (.toRotationMatrix

          (doto (Quaternion.)

            (.fromAngleAxis (/ Math/PI 2)

                            Vector3f/UNIT_X)))

         x-move

         (doto (Matrix3f.)

           (.fromStartEndVectors Vector3f/UNIT_X

                                 (.normalize (Vector3f. 1 1 0))))

         

         timer (atom 0)]

     (doto 

         (ConeJoint.

          control-a control-b

          (Vector3f. -8 0 0)

          (Vector3f. 2 0 0)

          ;;swivel swivel

          ;;Matrix3f/IDENTITY Matrix3f/IDENTITY

          x-move Matrix3f/IDENTITY 

          )

       (.setCollisionBetweenLinkedBodys false)

       (.setLimit (* 1 (/ Math/PI 4))    ;; twist 

                  (* 1 (/ Math/PI 4))  ;; swing span in X-Y plane

                  (* 0 (/ Math/PI 4))))  ;; swing span in Y-Z plane

     (world (nodify

             [obj-a obj-b])

            (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]

              (enable-debug world)

              (set-gravity world Vector3f/ZERO)

              )

                            

            (fn [world _]

 

              (if @move-up?

                (.applyForce control-a

                               (Vector3f. force 0 0)

                               (Vector3f. 0 0 0)))

              (if @move-down?

                (.applyForce control-a

                              (Vector3f. (- force) 0 0)

                              (Vector3f. 0 0 0)))

              (if @move-left?

                (.applyForce control-a

                              (Vector3f. 0 force 0)

                              (Vector3f. 0 0 0)))

              (if @move-right?

                (.applyForce control-a

                              (Vector3f. 0 (- force) 0)

                              (Vector3f. 0 0 0)))

 

              (if @roll-left?

                (.applyForce control-a

                              (Vector3f. 0 0 force)

                              (Vector3f. 0 0 0)))

              (if @roll-right?

                (.applyForce control-a

                              (Vector3f. 0 0 (- force))

                              (Vector3f. 0 0 0)))

 

              (if (zero? (rem (swap! timer inc) 100))

                (.attachChild

                 (.getRootNode world)

                 (sphere 0.05 :color ColorRGBA/Yellow

                         :physical? false :position

                         (.getWorldTranslation obj-a)))))

            )

     ))

 

 

 

 #+end_src

 

 

 * COMMENT generate source

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

 <<body-1>>

 #+end_src