before merge

script/imitator.py

@@ -20,11 +20,13 @@ if __name__ == '__main__':
     ll = tf.TransformListener()
     mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
     mp.wakeUp()
-    mp.setAngles(['LElbowRoll', 'RElbowRoll'],
-                 [radians(-50), radians(50)], 0.5)
+    mp.setAngles(['LElbowRoll', 'RElbowRoll', 'LElbowYaw', 'RElbowYaw'],
+                 [radians(-70), radians(70), -radians(40), radians(40)], 0.5)
 
     while not rospy.is_shutdown():
         sleep(0.1)
+        if not _inform_controller('imitate'):
+            continue
         for i, eff in enumerate(['L',
                                  'R'], 1):
             try:
@@ -36,9 +38,6 @@ if __name__ == '__main__':
             except Exception as e:
                 print e
                 continue
-            permission = _inform_controller('imitate')
-            if not permission:
-                continue
             sign = 1 if eff == 'L' else -1
             roll = asin(trans[1] /
                         sqrt(trans[0]**2 + trans[1]**2 + trans[2]**2))
@@ -48,6 +47,7 @@ if __name__ == '__main__':
             # sign = 1 if roll > 0 else -1
             # roll -= sign * radians(10)
             # print degrees(roll)
+
             mp.setAngles([
                 '{}ShoulderRoll'.format(eff),
                 '{}ShoulderPitch'.format(eff)
@@ -55,6 +55,7 @@ if __name__ == '__main__':
                 roll,
                 pitch
             ], 0.3)
+
             # targ = np.array(trans)
             # targ = targ / np.linalg.norm(targ) * 0.3
             # mp.setPositions(eff, _FRAME_TORSO,