Refactored kick

pykick/kick.py

@@ -1,99 +1,9 @@
-import time
-import argparse
-from naoqi import ALProxy
-from math import radians
-
-
-def main(robotIP, PORT=9559):
-    motionProxy = ALProxy("ALMotion", robotIP, PORT)
-
-    motionProxy.setStiffnesses("Head", 1.0)
-    motionProxy.setStiffnesses("LShoulderRoll",0.9)
-    motionProxy.setStiffnesses("LHipPitch",0.9)
-    # Example showing how to set angles, using a fraction of max speed
-    #names  = ["HeadYaw", "HeadPitch"]
-    #angles  = [0.2, -0.2]
-    
-    
-    fractionMaxSpeed = 0.5
-    names=["RShoulderRoll",]
-    
-    angles=[radians(-76)]
-    motionProxy.setAngles(names,angles,fractionMaxSpeed)
-    
-    time.sleep(3)
-   
-    fractionMaxSpeed  = 0.05
-    motionProxy.setStiffnesses("RAnkleRoll",0.9)
-    #names=["LHipRoll","RHipRoll","RAnkleRoll","LAnkleRoll"]
-    names=["RAnkleRoll","LAnkleRoll"]
-    #names=["LHipRoll","RHipRoll","LHipPitch","RAnkleRoll","LAnklePitch"]
-    angles=[radians(-10),radians(-10)]
-#   angles=[radians(-20),radians(-20),radians(-10),radians(-10)]
-#    angles=[radians(-20),radians(-20),radians(-15),radians(-5),radians(0)]
-    motionProxy.setAngles(names, angles, fractionMaxSpeed)
-
-    time.sleep(3)
-    fractionMaxSpeed=0.2
-    names=["LKneePitch","LAnklePitch"] 
-    angles=[radians(90),radians(-40)]
-    motionProxy.setAngles(names,angles,fractionMaxSpeed)
-    
-    time.sleep(3)
-    names=["LHipPitch"]
-    angles=[radians(-45)]
-    fractionMaxSpeed=0.01
-    motionProxy.setAngles(names,angles,fractionMaxSpeed)
-    names=["LKneePitch","LAnklePitch"]
-    angles=[radians(20),radians(-5)]
-    fractionMaxSpeed=0.05
-    motionProxy.setAngles(names,angles,fractionMaxSpeed)
-    
-    '''
-    time.sleep(3)
-    names=["LShoulderRoll"]
-    angles=[radians(76)]
-    motionProxy.setAngles(names,angles,fractionMaxSpeed)
-    '''
-   
-    '''
-    time.sleep(0.5)
-    names=["LAnklePitch"]
-    angles=[radians(-20)]
-    motionProxy.setAngles(names,angles,fractionMaxSpeed)
-
-    '''
-
-    
-#    time.sleep(4)
-#    print("kick")
-#    names=["LKneePitch","LAnklePitch"]
-#    angles=[radians(10),radians(0)]
-#    motionProxy.setAngles(names,angles,fractionMaxSpeed)
-
-    '''
-    #motionProxy.waitUntilMoveIsFinished()
-    time.sleep(6)
-    
-    motionProxy.setStiffnesses("RAnklePitch",0.9)
-    names=["RKneePitch","RHipPitch","LAnklePitch"]
-    angles=[radians(45),radians(-50),radians(-45)]
-
-    motionProxy.setAngles(names, angles, fractionMaxSpeed)
-    '''
-    
-    #time.sleep(3.0)
-    #motionProxy.setStiffnesses("Head", 0.0)
+from .utils import read_config
+from .movements import NaoMover
 
 
 if __name__ == "__main__":
-    parser = argparse.ArgumentParser()
-    parser.add_argument("--ip", type=str, default="192.168.0.11",
-                        help="Robot ip address")
-    parser.add_argument("--port", type=int, default=9559,
-                        help="Robot port number")
-
-    args = parser.parse_args()
-
-    main(args.ip, args.port)
-    
+    cfg = read_config()
+    mover = NaoMover(cfg['ip'])
+    mover.stand_up()
+    mover.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)