moved speech detection into controller

also the thing works quite good (as in demo-good)

script/imitator.py

@@ -1,17 +1,27 @@
 #! /usr/bin/env python
 import os
+import json
 from time import sleep
 from math import atan, asin, radians, sqrt
 
 import rospy
 import tf
+import numpy as np
 from naoqi import ALProxy
 
 from controller import inform_controller_factory
 
 
 _inform_controller = inform_controller_factory('imitator')
+
+
 _FRAME_TORSO = 0
+arm = None
+
+
+def _elbow(arm_):
+    quot = min(1.0, arm_ / arm)
+    return radians(180 * (1 - quot))
 
 
 if __name__ == '__main__':
@@ -25,6 +35,12 @@ if __name__ == '__main__':
     mp.setAngles(['LElbowRoll', 'RElbowRoll', 'LElbowYaw', 'RElbowYaw'],
                  [radians(-70), radians(70), -radians(40), radians(40)], 0.5)
 
+    here = os.path.dirname(os.path.realpath(__file__))
+    with open('{}/../config/default.yaml'.format(here)) as f:
+        x = json.load(f)
+
+    arm = x['arm']
+
     while not rospy.is_shutdown():
         sleep(0.1)
         if not _inform_controller('imitate'):
@@ -63,12 +79,16 @@ if __name__ == '__main__':
             roll -= sign * radians(25)
             pitch = atan(-trans[2] / abs(trans[0]))
 
+            eroll = -sign * _elbow(np.linalg.norm(np.array(trans)))
+
             mp.setAngles([
                 '{}ShoulderRoll'.format(eff),
-                '{}ShoulderPitch'.format(eff)
+                '{}ShoulderPitch'.format(eff),
+                '{}ElbowRoll'.format(eff)
             ], [
                 roll,
-                pitch
+                pitch,
+                eroll
             ], 0.3)
 
             # targ = np.array(trans)