refactored the code, fixed config bug

script/controller.py

@@ -5,12 +5,12 @@ import json
 from math import asin, atan, radians
 
 import numpy as np
-from naoqi import ALProxy
+
+from masterloop import mp
 
 
 FRAME_TORSO = 0
 K = 0.1
-mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
 
 _here = os.path.dirname(os.path.realpath(__file__))
 with open('{}/../config/default.yaml'.format(_here)) as f:
@@ -63,7 +63,7 @@ def our_cartesian(my_xyz, side):
     nao_xyz = to_nao(my_xyz, side)
     delta_r = K * (nao_xyz - xyz('{}Arm'.format(side)))
     crt_q = mp.getAngles([side + j for j in JOINTS], False)
-    delta_q =  np.linalg.pinv(jacobian()).dot(delta_r).flatten()
+    delta_q =  np.linalg.pinv(jacobian(side)).dot(delta_r).flatten()
     return crt_q + delta_q
 
 

script/fall_detector.py

@@ -4,7 +4,7 @@ import os
 import rospy
 from naoqi import ALProxy, ALBroker, ALModule
 
-from masterloop import inform_masterloop_factory
+from masterloop import inform_masterloop_factory, mp
 
 
 fall_broker = None
@@ -18,7 +18,6 @@ class FallDetectorModule(ALModule):
 
     def __init__(self, name):
         ALModule.__init__(self, name)
-        self.mp = ALProxy('ALMotion')
 
     def on_robot_falling(self, *_args):
         _inform_masterloop('falling')
@@ -26,9 +25,11 @@ class FallDetectorModule(ALModule):
     def on_robot_fallen(self, *_args):
         if not _inform_masterloop('fallen'):
             return
-        self.mp.rest()
+        mp.rest()
         rospy.Rate(0.5).sleep()
-        self.mp.wakeUp()
+        mp.wakeUp()
+        am = ALProxy('ALAutonomousMoves', os.environ['NAO_IP'], 9559)
+        am.setExpressiveListeningEnabled(False)
         pp = ALProxy('ALRobotPosture')
         while not pp.goToPosture('StandInit', 1.0):
             pass

script/imitator.py

@@ -1,12 +1,10 @@
 #! /usr/bin/env python
-import os
 
 import rospy
 import tf
-from naoqi import ALProxy
 
 from masterloop import inform_masterloop_factory
-from controller import movement, dumb, mp
+from controller import movement, dumb, our_cartesian
 
 
 _inform_masterloop = inform_masterloop_factory('imitator')
@@ -21,27 +19,22 @@ if __name__ == '__main__':
 
     ll = tf.TransformListener()
 
-    am = ALProxy('ALAutonomousMoves', os.environ['NAO_IP'], 9559)
-    am.setExpressiveListeningEnabled(False)
-
-    mp.wakeUp()
-
     while not rospy.is_shutdown():
         rospy.Rate(10).sleep()
         if not _inform_masterloop('imitate'):
             continue
         rospy.logdebug('IMITATOR: ACTIVE')
 
-        if TORSO:
-            try:
-                _, q = ll.lookupTransform('odom',
-                                          'Aruco_0_frame',
-                                          rospy.Time(0))
-                rot = tf.transformations.euler_from_quaternion(q)
-                mp.setAngles(['LHipYawPitch', 'RHipYawPitch'],
-                             [-rot[1], -rot[1]], 0.3)
-            except Exception as e:
-                rospy.logwarn(e)
+        # if TORSO:
+            # try:
+                # _, q = ll.lookupTransform('odom',
+                                          # 'Aruco_0_frame',
+                                          # rospy.Time(0))
+                # rot = tf.transformations.euler_from_quaternion(q)
+                # mp.setAngles(['LHipYawPitch', 'RHipYawPitch'],
+                             # [-rot[1], -rot[1]], 0.3)
+            # except Exception as e:
+                # rospy.logwarn(e)
 
         for i, side in enumerate(['L', 'R'], 1):
             try:
@@ -54,6 +47,4 @@ if __name__ == '__main__':
                 rospy.logwarn(e)
                 continue
 
-            movement(my_arm_xyz, side, dumb)
-
-    mp.rest()
+            movement(my_arm_xyz, side, our_cartesian)

script/masterloop.py

@@ -1,4 +1,5 @@
 #! /usr/bin/env python
+import os
 from argparse import ArgumentParser
 
 import rospy
@@ -7,6 +8,7 @@ import actionlib
 from teleoperation.srv import InformMasterloop, InformMasterloopResponse
 from teleoperation.srv import Hands
 from teleoperation.msg import RequestSpeechAction, RequestSpeechGoal
+from naoqi import ALProxy
 
 
 # Constants
@@ -32,6 +34,8 @@ state = 'dead'  # Also walk, imitate, fallen, idle
 hand_state = 'closed'
 speech_in_progress = False
 
+mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
+
 
 def init_voc_state_speech():
     global VOC_STATE, SPEECH_TRANSITIONS
@@ -144,15 +148,24 @@ if __name__ == '__main__':
     AI = args.autoimitate
     init_voc_state_speech()
 
-    ic = rospy.Service('inform_masterloop', InformMasterloop, handle_request)
-
     speech = actionlib.SimpleActionClient('speech_server',
                                           RequestSpeechAction)
-
     speech.wait_for_server()
     rospy.loginfo('SPEECH: SERVER THERE')
 
-    rospy.on_shutdown(lambda: speech_in_progress and speech.cancel_goal())
+    mp.wakeUp()
+    mp.moveInit()
+    am = ALProxy('ALAutonomousMoves', os.environ['NAO_IP'], 9559)
+    am.setExpressiveListeningEnabled(False)
+
+    ic = rospy.Service('inform_masterloop', InformMasterloop, handle_request)
+
+    def _shutdown():
+        if speech_in_progress:
+            speech.cancel_goal()
+        mp.rest()
+
+    rospy.on_shutdown(_shutdown)
 
     while not rospy.is_shutdown():
         if state in ('idle', 'imitate', 'dead'):

script/walker.py

@@ -5,9 +5,8 @@ from time import sleep
 
 import rospy
 import tf
-from naoqi import ALProxy
 
-from masterloop import inform_masterloop_factory
+from masterloop import inform_masterloop_factory, mp
 
 
 FW = None
@@ -60,9 +59,6 @@ if __name__ == '__main__':
     global_init()
     rospy.wait_for_service('inform_masterloop')
     ll = tf.TransformListener()
-    mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
-    mp.wakeUp()
-    mp.moveInit()
     mp.setMoveArmsEnabled(False, False)
 
     while not rospy.is_shutdown():