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@406: 
rlm@406: (use 'clojure.pprint)
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@406:   [muscle-labels
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@406:                     (map #(@current-forces % 0) muscle-labels)))))))
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@406:   (mapcat gen-wiggle (repeat 4000 [:a-down :a-up])
rlm@406:                      (range 100 1000000 (+ 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@405: 
rlm@405: (defn straight?
rlm@405:   "Is the worm straight?"
rlm@405:   [experiences]
rlm@405:   (every?
rlm@405:    (fn [[_ _ bend]]
rlm@405:      (< (Math/sin bend) 0.05))
rlm@405:    (:proprioception (peek experiences))))
rlm@405: 
rlm@405: (defn curled?
rlm@405:   "Is the worm curled up?"
rlm@405:   [experiences]
rlm@405:   (every?
rlm@405:    (fn [[_ _ bend]]
rlm@405:      (> (Math/sin bend) 0.64))
rlm@405:    (:proprioception (peek experiences))))
rlm@405: 
rlm@405: (defn grand-circle?
rlm@405:   "Does the worm form a majestic circle (one end touching the other)?"
rlm@405:   [experiences]
rlm@405:   (and (curled? experiences)
rlm@406:        true)) ;; TODO: add code here.
rlm@405: 
rlm@405: (defn vector:last-n [v n]
rlm@405:   (let [c (count v)]
rlm@405:     (if (< c n) v
rlm@405:         (subvec v (- c n) c))))
rlm@400: 
rlm@406: (defn touch-average [[coords touch]]
rlm@406:   (/ (average (map first touch)) (average (map second touch))))
rlm@406: 
rlm@407: (def worm-segment-touch-bottom
rlm@407:   [[8 15] [8 16] [8 17] [8 18] [8 19] [8 20] [8 21] [8 22] [9 15]
rlm@407:    [9 16] [9 17] [9 18] [9 19] [9 20] [9 21] [9 22] [10 15] [10 16]
rlm@407:    [10 17] [10 18] [10 19] [10 20] [10 21] [10 22] [11 15] [11 16]
rlm@407:    [11 17] [11 18] [11 19] [11 20] [11 21] [11 22] [12 15] [12 16]
rlm@407:    [12 17] [12 18] [12 19] [12 20] [12 21] [12 22] [13 15] [13 16]
rlm@407:    [13 17] [13 18] [13 19] [13 20] [13 21] [13 22] [14 15] [14 16]
rlm@407:    [14 17] [14 18] [14 19] [14 20] [14 21] [14 22]])
rlm@407: 
rlm@407: 
rlm@407: 
rlm@406: (defn floor-contact [[coords contact :as touch]]
rlm@406:   (let [raw-average
rlm@406:         (average
rlm@406:          (map
rlm@406:           first
rlm@406:           (vals
rlm@406:            (select-keys
rlm@406:             (zipmap coords contact)
rlm@407:             ))))]
rlm@406:     (Math/abs (- 1. (* 10 raw-average)))))
rlm@406: 
rlm@406: 
rlm@400: (defn wiggling?
rlm@405:   "Is the worm wiggling?"
rlm@405:   [experiences]
rlm@406:   (vector:last-n experiences 200)
rlm@405:   
rlm@406: )
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@405:      :record nil
rlm@407:      :experiences nil
rlm@407:      :worm-model worm-model
rlm@407:      :end-frame nil}))
rlm@407: 
rlm@404: 
rlm@404: (defn dir! [file]
rlm@404:   (if (not (.exists file))
rlm@404:     (.mkdir file))
rlm@404:   file)
rlm@405: 
rlm@405: (defn record-experience! [experiences data]
rlm@405:   (swap! experiences #(conj % data)))
rlm@405: 
rlm@399: (defn worm-world
rlm@407:   [& {:keys [record motor-control keybindings view experiences
rlm@407:              worm-model end-frame] :as settings}]
rlm@407:   (let [{:keys [record motor-control keybindings view experiences
rlm@407:                 worm-model end-frame]}
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@407:                    :color ColorRGBA/Gray :mass 0)
rlm@407:         timer (IsoTimer. 60)]
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@407:          (.setTimer world timer)
rlm@407:          (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@407:          (if (> (.getTime timer) end-frame)
rlm@407:            (.stop world))
rlm@405:          (let [muscle-data (motor-control muscles)
rlm@405:                proprioception-data (prop)
rlm@405:                touch-data (map #(% (.getRootNode world)) touch)]
rlm@405:            (when experiences
rlm@405:              (record-experience!
rlm@405:               experiences {:touch touch-data
rlm@405:                            :proprioception proprioception-data
rlm@405:                            :muscle muscle-data})
rlm@405:              (if (curled? @experiences) (println "Curled"))
rlm@406:              ;;(if (straight? @experiences)    (println "Straight"))
rlm@407:              ;; (println-repl
rlm@407:              ;;  (apply format "%.2f %.2f %.2f %.2f %.2f\n"
rlm@407:              ;;         (map floor-contact touch-data)))
rlm@407:              
rlm@405:              )
rlm@404:            (muscle-display
rlm@405:             muscle-data
rlm@405:             (if record (dir! (File. record "muscle"))))
rlm@405:            (prop-display
rlm@405:             proprioception-data
rlm@405:             (if record (dir! (File. record "proprio"))))
rlm@405:            (touch-display 
rlm@405:             touch-data
rlm@405:             (if record (dir! (File. record "touch")))))))))
rlm@407: 
rlm@407: 
rlm@407: ;; A demonstration of self organiging touch maps through experience. 
rlm@407: 
rlm@407: (def single-worm-segment-view
rlm@407:   [(Vector3f. 2.0681207, -6.1406755, 1.6106138)
rlm@407:    (Quaternion. -0.15558705, 0.843615, -0.3428654, -0.38281822)])
rlm@407: 
rlm@407: (def worm-single-segment-muscle-labels
rlm@407:   [:lift-1 :lift-2 :roll-1 :roll-2])
rlm@407: 
rlm@407: (defn touch-kinesthetics []
rlm@407:   [[170 :lift-1 40]
rlm@407:    [190 :lift-1 20]
rlm@407:    [206 :lift-1  0]
rlm@407: 
rlm@407:    [400 :lift-2 40]
rlm@407:    [410 :lift-2  0]
rlm@407: 
rlm@407:    [570 :lift-2 40]
rlm@407:    [590 :lift-2 20]
rlm@407:    [606 :lift-2  0]
rlm@407: 
rlm@407:    [800 :lift-1 40]
rlm@407:    [809 :lift-1 0]
rlm@407: 
rlm@407:    [900 :roll-2 40]
rlm@407:    [905 :roll-2 20]
rlm@407:    [910 :roll-2  0]
rlm@407: 
rlm@407:    [1000 :roll-2 40]
rlm@407:    [1005 :roll-2 20]
rlm@407:    [1010 :roll-2  0]
rlm@407:    
rlm@407:    [1100 :roll-2 40]
rlm@407:    [1105 :roll-2 20]
rlm@407:    [1110 :roll-2  0]
rlm@407:    ])
rlm@407: 
rlm@407: (defn worm-segment-defaults []
rlm@407:   (let [direct-control (worm-direct-control worm-muscle-labels 40)]
rlm@407:     (merge (worm-world-defaults)
rlm@407:            {:worm-model single-worm-segment
rlm@407:             :view single-worm-segment-view
rlm@407:             :motor-control
rlm@407:             (motor-control-program
rlm@407:              worm-single-segment-muscle-labels
rlm@407:              (touch-kinesthetics))})))
rlm@407: 
rlm@407: (defn single-worm-segment []
rlm@407:   (load-blender-model "Models/worm/worm-single-segment.blend"))
rlm@407: 
rlm@407: 
rlm@407: (defn pure-touch?
rlm@407:   "This is worm specific code to determine if a large region of touch
rlm@407:    sensors is either all on or all off."
rlm@407:   [[coords touch :as touch-data]]
rlm@407:   (= (set (map first touch)) #{(float 0.1)  (float 0.0)}))