# HG changeset patch
# User Robert McIntyre <rlm@mit.edu>
# Date 1393992283 18000
# Node ID 85393ec986dc9c6d50160cd111c2bdc5f543c1c5
# Parent  36d492f4deaba9910780174cf54b9d275ade00b3
completed action definitions.

diff -r 36d492f4deab -r 85393ec986dc org/worm_learn.clj
--- a/org/worm_learn.clj	Tue Mar 04 00:01:38 2014 -0500
+++ b/org/worm_learn.clj	Tue Mar 04 23:04:43 2014 -0500
@@ -15,73 +15,121 @@
 (rlm.rlm-commands/help)
 
 
-(defn worm-segment []
-  (load-blender-model "Models/worm-segment/worm-segment.blend"))
+(def hand "Models/test-creature/hand.blend")
 
+(defn worm-model []
+  (load-blender-model "Models/worm/worm.blend"))
 
-(defn worm []
-  (load-blender-model "Models/worm-segment/worm.blend"))
+(def output-base (File. "/home/r/proj/cortex/render/worm-learn/"))
 
 
-(defn gen-worm
-  "create a creature acceptable for testing as a replacement for the
-   worm."
-  []
-  (nodify
-   "worm"
-   [(nodify
-     "eyes"
-     [(doto
-          (Node. "eye1")
-        (.setLocalTranslation (Vector3f. 0 -1.1 0))
-        (.setUserData
-         
-         "eye" 
-         "(let [retina
-                \"Models/test-creature/retina-small.png\"]
-                {:all retina :red retina
-                 :green retina :blue retina})"))])
-    (box
-     0.2 0.2 0.2
-     :name "worm-segment"
-     :position (Vector3f. 0 0 0)
-     :color ColorRGBA/Orange)]))
+(defn motor-control-program
+  "Create a function which will execute the motor script"
+  [muscle-positions
+   script]
+  (let [current-frame (atom -1)
+        keyed-script (group-by first script)
+        current-forces (atom {}) ]
+    (fn [effectors]
+      (let [indexed-effectors (vec effectors)]
+        (dorun 
+         (for [[_ part force] (keyed-script (swap! current-frame inc))]
+           (swap! current-forces (fn [m] (assoc m part force)))))
+        (doall (map (fn [effector power]
+                      (effector (int power)))
+                    effectors
+                    (map #(@current-forces % 0) muscle-positions)))))))
 
+(def muscle-labels
+  [:base-up :base-down
+   :a-up :a-down
+   :b-up :b-down
+   :c-up :c-down
+   :d-up :d-down
+   ])
 
-(defn test-basic-touch
-  "Testing touch:
-   You should see a cube fall onto a table.  There is a cross-shaped
-   display which reports the cube's sensation of touch. This display
-   should change when the cube hits the table, and whenever you hit
-   the cube with balls.
+(def curl-script
+  [[370 :d-up 20]
+   [390 :d-up 0]])
 
-   Keys:
-     <space> : fire ball"
-  ([] (test-basic-touch false))
-  ([record?]
-     (let [head (doto (worm) (body!))
-           touch (touch! head)
-           touch-display (view-touch)]
+(def period 18)
+
+(defn gen-wiggle [[flexor extensor :as muscle-pair] time-base]
+  (let [period period
+        power 45]
+    [[time-base flexor power]
+     [(+ time-base period) flexor 0]
+     [(+ time-base period 1) extensor power]
+     [(+ time-base (+ (* 2 period) 2))  extensor 0]]))
+  
+(def wiggle-script
+  (mapcat gen-wiggle [[:d-up :d-down]
+                      [:c-up :c-down]
+                      [:b-up :b-down]
+                      [:a-up :a-down]] (range 100 1000 12)))
+
+(def wiggle-script
+  (mapcat gen-wiggle (repeat 40 [:a-down :a-up])
+                     (range 100 10000 (+ 3 (* period 2)))))
+
+
+
+(defn worm-world
+  ""
+  ([] (worm-world curl-script))
+  ([motion-script]
+     (let [record? false
+           worm (doto (worm-model) (body!))
+           touch   '();;(touch! worm)
+           prop    (proprioception! worm)
+           muscles (movement! worm)
+           
+           touch-display  (view-touch)
+           prop-display   (view-proprioception)
+           muscle-display (view-movement)
+
+           floor (box 10 1 10 :position (Vector3f. 0 -10 0)
+                      :color ColorRGBA/Gray :mass 0)
+
+           control-script (motor-control-program
+                           muscle-labels motion-script)]
        (world
-        (nodify [head
-                 (box 10 1 10 :position (Vector3f. 0 -10 0)
-                      :color ColorRGBA/Gray :mass 0)])
-        
+        (nodify [worm floor])
         standard-debug-controls
         
         (fn [world]
+          ;; (set-gravity world Vector3f/ZERO)
+          ;; (position-camera
+          ;;  world (Vector3f. 4.207176, -3.7366982, 3.0816958)
+          ;;  (Quaternion. 0.11118768, 0.87678415, 0.24434438, -0.3989771))
+
+
+
+          (position-camera
+           world (Vector3f. 4.207176, -3.7366982, 3.0816958)
+           (Quaternion. -0.11555642, 0.88188726, -0.2854942, -0.3569518))
+
+
+
+
           (let [timer (IsoTimer. 60)]
             (.setTimer world timer)
             (display-dilated-time world timer))
           (if record?
             (Capture/captureVideo
              world
-             (File. "/home/r/proj/cortex/render/touch-cube/main-view/")))
+             (File. output-base "main-view")))
           (speed-up world)
           (light-up-everything world))
         
         (fn [world tpf]
+          (muscle-display
+           (control-script muscles)
+           (if record? (File. output-base "muscle")))
+          (prop-display
+           (prop)
+           (if record? (File. output-base "proprio")))             
           (touch-display 
            (map #(% (.getRootNode world)) touch)
            (if record?
-             (File. "/home/r/proj/cortex/render/touch-cube/touch/"))))))))
+             (File. output-base "touch"))))))))