rlm@394: (ns org.aurellem.worm-learn
rlm@394:   "General worm creation framework."
rlm@394:   {:author "Robert McIntyre"}
rlm@394:   (:use (cortex world util import body sense
rlm@394:                 hearing touch vision proprioception movement))
rlm@394:   (:import (com.jme3.math ColorRGBA Vector3f))
rlm@394:   (:import java.io.File)
rlm@394:   (:import com.jme3.audio.AudioNode)
rlm@397:   (:import com.aurellem.capture.RatchetTimer)
rlm@397:   (:import (com.aurellem.capture Capture IsoTimer))
rlm@397:   (:import (com.jme3.math Vector3f ColorRGBA)))
rlm@397:   
rlm@394: 
rlm@394: (dorun (cortex.import/mega-import-jme3))
rlm@394: (rlm.rlm-commands/help)
rlm@394: 
rlm@400: (load-bullet)
rlm@394: 
rlm@399: (def hand "Models/test-creature/hand.blend")
rlm@394: 
rlm@399: (defn worm-model []
rlm@399:   (load-blender-model "Models/worm/worm.blend"))
rlm@394: 
rlm@400: (def output-base (File. "/home/r/proj/cortex/render/worm-learn/curl"))
rlm@394: 
rlm@397: 
rlm@399: (defn motor-control-program
rlm@399:   "Create a function which will execute the motor script"
rlm@399:   [muscle-positions
rlm@399:    script]
rlm@399:   (let [current-frame (atom -1)
rlm@399:         keyed-script (group-by first script)
rlm@399:         current-forces (atom {}) ]
rlm@399:     (fn [effectors]
rlm@399:       (let [indexed-effectors (vec effectors)]
rlm@399:         (dorun 
rlm@399:          (for [[_ part force] (keyed-script (swap! current-frame inc))]
rlm@399:            (swap! current-forces (fn [m] (assoc m part force)))))
rlm@399:         (doall (map (fn [effector power]
rlm@399:                       (effector (int power)))
rlm@399:                     effectors
rlm@399:                     (map #(@current-forces % 0) muscle-positions)))))))
rlm@397: 
rlm@404: (defn worm-direct-control
rlm@404:   "Create keybindings and a muscle control program that will enable
rlm@404:    the user to control the worm via the keyboard."
rlm@404:   [muscle-labels activation-strength]
rlm@404:   (let [strengths (mapv (fn [_] (atom 0)) muscle-labels)
rlm@404:         activator
rlm@404:         (fn [n]
rlm@404:           (fn [world pressed?]
rlm@404:             (let [strength (if pressed? activation-strength 0)]
rlm@404:               (swap! (nth strengths n) (constantly strength)))))
rlm@404:         activators
rlm@404:         (map activator (range (count muscle-labels)))
rlm@404:         worm-keys
rlm@404:         ["key-f" "key-r"
rlm@404:          "key-g" "key-t"
rlm@404:          "key-y" "key-h"
rlm@404:          "key-j" "key-u"
rlm@404:          "key-i" "key-k"
rlm@404:          "key-o" "key-l"]]
rlm@404:     {:motor-control
rlm@404:      (fn [effectors]
rlm@404:       (doall
rlm@404:        (map (fn [strength effector]
rlm@404:               (effector (deref strength)))
rlm@404:             strengths effectors)))
rlm@404:      :keybindings
rlm@404:      ;; assume muscles are listed in pairs and map them to keys.
rlm@404:      (zipmap worm-keys activators)}))
rlm@400: 
rlm@400: ;; These are scripts that direct the worm to move in two radically
rlm@400: ;; different patterns -- a sinusoidal wiggling motion, and a curling
rlm@400: ;; motions that causes the worm to form a circle.
rlm@400: 
rlm@400: (def curl-script
rlm@400:   [[370 :d-up 40]
rlm@400:    [600 :d-up 0]])
rlm@400: 
rlm@400: (def period 18)
rlm@400: 
rlm@404: (def worm-muscle-labels
rlm@399:   [:base-up :base-down
rlm@404:    :a-down :a-up
rlm@399:    :b-up :b-down
rlm@404:    :c-down :c-up
rlm@400:    :d-up :d-down])
rlm@399: 
rlm@399: (defn gen-wiggle [[flexor extensor :as muscle-pair] time-base]
rlm@399:   (let [period period
rlm@399:         power 45]
rlm@399:     [[time-base flexor power]
rlm@399:      [(+ time-base period) flexor 0]
rlm@399:      [(+ time-base period 1) extensor power]
rlm@399:      [(+ time-base (+ (* 2 period) 2))  extensor 0]]))
rlm@399:   
rlm@399: (def wiggle-script
rlm@399:   (mapcat gen-wiggle (repeat 40 [:a-down :a-up])
rlm@399:                      (range 100 10000 (+ 3 (* period 2)))))
rlm@399: 
rlm@399: 
rlm@400: ;; Normally, we'd use unsupervised/supervised machine learning to pick
rlm@400: ;; out the defining features of the different actions available to the
rlm@400: ;; worm. For this project, I am going to explicitely define functions
rlm@400: ;; that recognize curling and wiggling respectively. These functions
rlm@400: ;; are defined using all the information available from an embodied
rlm@400: ;; simulation of the action. Note how much easier they are to define
rlm@400: ;; than if I only had vision to work with. Things like scale/position
rlm@400: ;; invariance are complete non-issues here. This is the advantage of
rlm@400: ;; body-centered action recognition and what I hope to show with this
rlm@400: ;; thesis.
rlm@400: 
rlm@400: (defn last-nth
rlm@400:   "Create function that will, when called each frame with the senses
rlm@400:    of a creature, will record those results and return the last n
rlm@400:    results."
rlm@400:   [n]
rlm@400:   (let [last-n  '()]
rlm@400:     (fn [frame-num {:keys [touch proprioception muscles hearing]}]
rlm@400:       (take n (cons [frame-num :stuff] last-n)))))
rlm@400:       
rlm@400: 
rlm@400: (defn wiggling?
rlm@400:   "Generate a function which, when called each frame with the sensory
rlm@400:    inputs of a worm, will determine whether the worm is wiggling."
rlm@400:   [{:keys [touch proprioception muscles hearing]}]
rlm@400:   (map (fn [f] (f)) proprioception
rlm@400:    
rlm@400: 
rlm@400: ))
rlm@400: 
rlm@400: (defn curling?
rlm@400:   "Is the worm curled up?"
rlm@400:   []
rlm@400:   )
rlm@400: 
rlm@400: (defn resting?
rlm@400:   "Is the worm on the ground in a neutral position?"
rlm@400:   [])
rlm@400: 
rlm@400: (def standard-world-view
rlm@400:   [(Vector3f. 4.207176, -3.7366982, 3.0816958)
rlm@400:    (Quaternion. 0.11118768, 0.87678415, 0.24434438, -0.3989771)])
rlm@400: 
rlm@400: (def worm-side-view
rlm@400:   [(Vector3f. 4.207176, -3.7366982, 3.0816958)
rlm@400:    (Quaternion. -0.11555642, 0.88188726, -0.2854942, -0.3569518)])
rlm@400: 
rlm@400: (def degenerate-worm-view
rlm@400:   [(Vector3f. -0.0708936, -8.570261, 2.6487997)
rlm@400:    (Quaternion. -2.318909E-4, 0.9985348, 0.053941682, 0.004291452)])
rlm@399: 
rlm@404: (defn worm-world-defaults []
rlm@404:   (let [direct-control (worm-direct-control worm-muscle-labels 40)]
rlm@404:     {:view worm-side-view
rlm@404:      :motor-control (:motor-control direct-control)
rlm@404:      :keybindings (:keybindings direct-control)
rlm@404:      :record nil}))
rlm@404: 
rlm@404: (defn dir! [file]
rlm@404:   (if (not (.exists file))
rlm@404:     (.mkdir file))
rlm@404:   file)
rlm@404:   
rlm@399: (defn worm-world
rlm@404:   [& {:keys [record motor-control keybindings view] :as settings}]
rlm@404:   (let [{:keys [record motor-control keybindings view]}
rlm@404:         (merge (worm-world-defaults) settings)
rlm@404:         worm (doto (worm-model) (body!))
rlm@404:         touch   (touch! worm)
rlm@404:         prop    (proprioception! worm)
rlm@404:         muscles (movement! worm)
rlm@404:         
rlm@404:         touch-display  (view-touch)
rlm@404:         prop-display   (view-proprioception)
rlm@404:         muscle-display (view-movement)
rlm@404:         
rlm@404:         floor (box 10 1 10 :position (Vector3f. 0 -10 0)
rlm@404:                    :color ColorRGBA/Gray :mass 0)]
rlm@399: 
rlm@404:     (world
rlm@404:        (nodify [worm floor])
rlm@404:        (merge standard-debug-controls keybindings)
rlm@404:        (fn [world]
rlm@404:          (position-camera world view)
rlm@404:          (let [timer (IsoTimer. 60)]
rlm@404:            (.setTimer world timer)
rlm@404:            (display-dilated-time world timer))
rlm@404:          (if record
rlm@404:            (Capture/captureVideo
rlm@404:             world
rlm@404:             (dir! (File. record "main-view"))))
rlm@404:          (speed-up world)
rlm@404:          (light-up-everything world))
rlm@404:        (fn [world tpf]
rlm@404:          (let [strong! (motor-control muscles)]
rlm@404:            (println strong!)
rlm@404:            (muscle-display
rlm@404:             strong!
rlm@404:             (if record (dir! (File. record "muscle")))))
rlm@404:          (prop-display
rlm@404:           (prop)
rlm@404:           (if record (dir! (File. record "proprio"))))
rlm@404:          (touch-display 
rlm@404:           (map #(% (.getRootNode world)) touch)
rlm@404:           (if record
rlm@404:             (File. record "touch")))))))
rlm@399: 
rlm@404: