started with imitation

script/controller.py

@@ -44,6 +44,18 @@ def handle_request(r):
         elif message == 'recovered':
             STATE = 'idle'
 
+    elif module == 'imitator':
+        if message == 'imitate':
+            if STATE in ('imitate', 'idle'):
+                STATE = 'imitate'
+                permission = True
+            else:
+                permission = False
+        elif message == 'stop':
+            if STATE == 'imitate':
+                STATE = 'idle'
+            permission = True
+
     print 'Got request from %s to %s. Permission: %s. State is now: %s.' % (
         module, message, permission, STATE
     )

script/imitator.py

@@ -0,0 +1,56 @@
+#! /usr/bin/env python
+import os
+from time import sleep
+from math import atan, degrees, radians
+
+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
+
+
+if __name__ == '__main__':
+    rospy.init_node('imitator')
+    rospy.wait_for_service('inform_controller')
+    ll = tf.TransformListener()
+    mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
+    mp.wakeUp()
+    mp.setAngles(['LElbowRoll', 'RElbowRoll'], [0, 0], 0.5)
+
+    while not rospy.is_shutdown():
+        sleep(0.1)
+        for i, eff in enumerate(['L',
+                                 'R'], 1):
+            try:
+                trans, _ = ll.lookupTransform(
+                    'Aruco_0_frame',
+                    'Aruco_{}_frame'.format(i),
+                    rospy.Time(0)
+                )
+            except Exception as e:
+                print e
+                continue
+            permission = _inform_controller('imitate')
+            if not permission:
+                continue
+            roll = atan(trans[1] / abs(trans[0]))
+            pitch = atan(-trans[2] / abs(trans[0]))
+
+            # sign = 1 if roll > 0 else -1
+            # roll -= sign * radians(10)
+            print degrees(roll)
+            mp.setAngles(['{}ShoulderRoll'.format(eff),
+                          '{}ShoulderPitch'.format(eff)], [roll, pitch], 0.3)
+            # targ = np.array(trans)
+            # targ = targ / np.linalg.norm(targ) * 0.3
+            # mp.setPositions(eff, _FRAME_TORSO,
+                            # targ.tolist() + [0, 0, 0], 0.5, 7)
+            # print eff, targ
+
+    mp.rest()

script/walker.py

@@ -33,7 +33,7 @@ if __name__ == '__main__':
                                             rospy.Time(0))
         except Exception as e:
             mp.stopMove()
-            print _inform_controller('stop')
+            _inform_controller('stop')
             continue
 
         print trans, rot
@@ -44,8 +44,8 @@ if __name__ == '__main__':
             LT < trans[0] < RT
             # CW < trans[1] < CC
         ):
-            mp.move(0, 0, 0)
             _inform_controller('stop')
+            mp.move(0, 0, 0)
             continue
 
         permission = _inform_controller('move')