diff --git a/pykick/kick.py b/pykick/kick.py index b52f341..e0a9bac 100644 --- a/pykick/kick.py +++ b/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() diff --git a/pykick/movements.py b/pykick/movements.py index 2ba8173..63c7f8a 100644 --- a/pykick/movements.py +++ b/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)