started with imitation

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()