Refactored kick

pykick/movements.py

@@ -1,8 +1,25 @@
+from time import sleep
+from math import radians
+
 from naoqi import ALProxy
 
 
 class NaoMover(object):
 
+    KICK_SEQUENCE = [
+        (0, 'ShoulderRoll', -45, 0.5),
+        'wait',
+        (0, 'AnkleRoll', -10, 0.1),
+        (1, 'AnkleRoll', -10, 0.1),
+        'wait',
+        (1, 'KneePitch', 90, 0.2),
+        (1, 'AnklePitch', -40, 0.2),
+        'wait',
+        (1, 'HipPitch', -45, 0.03),
+        (1, 'KneePitch', 10, 0.05),
+        (1, 'AnklePitch', 20, 0.03)
+    ]
+
     def __init__(self, nao_ip, nao_port=9559):
         nao_ip = bytes(nao_ip)
         self.mp = ALProxy('ALMotion', nao_ip, nao_port)
@@ -18,6 +35,28 @@ class NaoMover(object):
         self.set_ankle_stiffness()
         self.ready_to_move = False
 
+    def kick(self, foot='L'):
+        self.set_arm_stiffness(0.8)
+        self.set_hip_stiffness(0.8)
+        self.set_knee_stiffness(0.8)
+        self.set_ankle_stiffness(0.8)
+        multiplier = 1
+        if foot == 'L':
+            sides = ['R', 'L']
+        elif foot == 'R':
+            sides = ['R', 'L']
+        reverse = []
+        for motion in self.KICK_SEQUENCE:
+            if motion == 'wait':
+                sleep(3)
+            else:
+                print(motion)
+                side, joint, angle, speed = motion
+                reverse.extend(self.mp.getAngles([sides[side] + joint])
+                self.mp.setAngles(
+                    [sides[side] + joint], [radians(angle)], speed * multiplier
+                )
+
     def stand_up(self):
         self.set_arm_stiffness(0.9)
         self.set_hand_stiffness(0.9)