NAO机器人抓红球

‘‘‘
###This code is to make the robot walk till the location of the red ball
### by going hald of the distance everytime and aligning the body position
### and kick the red ball with its legs
‘‘‘

from naoqi import *
import time math numpyalmathargparse
IP = ‘172.26.250.12‘   
PORT = 9559


#API CALLS
mp      = ALProxy（“ALMotion“ IP PORT） 
sonar   = ALProxy（“ALSonar“ IP PORT） 
mem     = ALProxy（“ALMemory“ IP PORT）
sonarProxy = ALProxy（“ALSonar“ IP PORT） 
red     = ALProxy（“ALRedBallDetection“ IP PORT）
track   = ALProxy（“ALRedBallTracker“ IP PORT）
tracker = ALProxy（“ALTracker“ IP PORT）
postureProxy = ALProxy（“ALRobotPosture“ IP PORT）


#Set the stiffness
def StiffnessOn（proxy）:
    pNames = “Body“
    pStiffnessLists = 1.0
    pTimeLists = 1.0
    proxy.stiffnessInterpolation（pNames pStiffnessLists pTimeLists）

#Move the face of the robot inorder to track the redball
def turn_and_find（prev_x prev_y）:
    YAWrange = [-50.52 50.52]
    PITCHrange = [-20 20] 
    pitchList = numpy.linspace（PITCHrange[0]PITCHrange[1]5）
    yawList = numpy.linspace（YAWrange[0]YAWrange[1]5）

    for i in yawList:
        for j in pitchList:
            Hp = j
            Hy = i
            head = [Hy Hp]
            # Gather the joints together
            targetAngles = head 
            # Convert to radians
            targetAngles = [x * motion.TO_RAD for x in targetAngles]     
            # We use the “Body“ name to signify the collection of all joints
            names = “Head“
            # Ask motion to do this with a blocking call
            mp.angleInterpolationWithSpeed（names targetAngles 0.1）
            memValue = ‘redBallDetected‘
            val = mem.getData（memValue 0）
            xyz = track.getPosition（）
            if x != prev_x or y != prev_y:
                print xyz
                return [xyz]

    print “No ball found“
    print 000
    return [000]

#Kick the ball using both left and right legs
def kick（）: 

    # Set NAO in Stiffness On
    StiffnessOn（mp）

    # Send NAO to Pose Init
    postureProxy.goToPosture（“StandInit“ 0.5）

    # Activate Whole Body Balancer
    isEnabled  = True
    mp.wbEnable（isEnabled）

    # Legs are constrained fixed
    stateName  = “Fixed“
    supportLeg = “Legs“
    mp.wbFootState（stateName supportLeg）

    # Constraint Balance Motion
    isEnable   = True
    supportLeg = “Legs“
    mp.wbEnableBalanceConstraint（isEnable supportLeg）

    # Com go to LLeg
    supportLeg = “LLeg“
    duration   = 2.0
    mp.wbGoToBalance（supportLeg duration）

    # RLeg is free
    stateName  = “Free“
    supportLeg = “RLeg“
    mp.wbFootState（stateName supportLeg）

    # RLeg is optimized
    effectorName = “RLeg“
    axisMask     = 63
    space        = motion.frame_TORSO   #ROBOT


    # Motion of the RLeg
    dx      = 0.1                   # translation axis X （meters）
    dz      = 0.1                   # translation axis Z （meters）
    dwy     = 5.0*math.pi/180.0     # rot