finalizing

script/imitator.py

@@ -1,12 +1,14 @@
 #! /usr/bin/env python
 from argparse import ArgumentParser
+from math import radians
 
 import rospy
 import tf
 import numpy as np
 
-from masterloop import inform_masterloop_factory
-from controller import nao_cart_movement, movement, dumb, our_cartesian
+from masterloop import inform_masterloop_factory, mp
+from controller import nao_cart_movement, movement
+from controller import dumb, our_cartesian, our_diff_cartesian
 
 
 _inform_masterloop = inform_masterloop_factory('imitator')
@@ -23,6 +25,8 @@ def controller_factory(ctrl):
         cfunc = dumb
     elif ctrl == 'our_cartesian':
         cfunc = our_cartesian
+    elif ctrl == 'our_diff_cartesian':
+        cfunc = our_diff_cartesian
     return lambda my_arm_xyz, side: movement(my_arm_xyz, side, cfunc)
 
 
@@ -30,7 +34,8 @@ if __name__ == '__main__':
     rospy.init_node('imitator')
     ap = ArgumentParser()
     ap.add_argument('--controller', default='dumb', const='dumb', nargs='?',
-                    choices=['dumb', 'nao_cartesian', 'our_cartesian'],
+                    choices=['dumb', 'nao_cartesian', 'our_cartesian',
+                             'our_diff_cartesian'],
                     help='Choose the controller for arm motions')
     args, _ = ap.parse_known_args()
     imitation_cycle = controller_factory(args.controller)
@@ -75,3 +80,5 @@ if __name__ == '__main__':
             # rospy.loginfo('{}'.format(my_arm_xyz))
             # rospy.loginfo('{}'.format(dumb(my_arm_xyz, side)))
             imitation_cycle(my_arm_xyz, side)
+            mp.setAngles(('LWristYaw', 'RWristYaw'),
+                         (radians(-70), radians(70)), 0.3)