did some good day's work

script/imitator.py

@@ -1,10 +1,12 @@
 #! /usr/bin/env python
+from argparse import ArgumentParser
 
 import rospy
 import tf
+import numpy as np
 
 from masterloop import inform_masterloop_factory
-from controller import movement, dumb, our_cartesian
+from controller import nao_cart_movement, movement, dumb, our_cartesian
 
 
 _inform_masterloop = inform_masterloop_factory('imitator')
@@ -20,7 +22,7 @@ if __name__ == '__main__':
     ll = tf.TransformListener()
 
     while not rospy.is_shutdown():
-        rospy.Rate(10).sleep()
+        rospy.Rate(20).sleep()
         if not _inform_masterloop('imitate'):
             continue
         rospy.logdebug('IMITATOR: ACTIVE')
@@ -38,13 +40,20 @@ if __name__ == '__main__':
 
         for i, side in enumerate(['L', 'R'], 1):
             try:
-                my_arm_xyz, _ = ll.lookupTransform(
+                a0, _ = ll.lookupTransform(
+                    'odom',
                     'Aruco_0_frame',
+                    rospy.Time(0)
+                )
+                arm, _ = ll.lookupTransform(
+                    'odom',
                     'Aruco_{}_frame'.format(i),
                     rospy.Time(0)
                 )
             except Exception as e:
                 rospy.logwarn(e)
                 continue
-
-            movement(my_arm_xyz, side, our_cartesian)
+            my_arm_xyz = np.array(arm) - np.array(a0)
+            # rospy.loginfo('{}'.format(my_arm_xyz))
+            # rospy.loginfo('{}'.format(dumb(my_arm_xyz, side)))
+            movement(my_arm_xyz, side, dumb)