‘‘‘
###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