def kickLeftFoot():# Kick ball with left foot. #Turns the stiffness on. config.stiffnessOn() motionProxy.angleInterpolationWithSpeed(pNames, left1(), 0.1) motionProxy.angleInterpolationWithSpeed(pNames, left2(), 0.3) motionProxy.angleInterpolationWithSpeed(pNames, left3(), 0.4) motionProxy.angleInterpolationWithSpeed(pNames, left4(), 1.0) motionProxy.angleInterpolationWithSpeed(pNames, left1(), 0.2)
def kickRightFoot():# Kick ball with right foot. #Turns the stiffness on. config.stiffnessOn() motionProxy.angleInterpolationWithSpeed(pNames, right1(), 0.1) motionProxy.angleInterpolationWithSpeed(pNames, right2(), 0.4) motionProxy.angleInterpolationWithSpeed(pNames, right3(), 0.4) motionProxy.angleInterpolationWithSpeed(pNames, right4(), 1.0) motionProxy.angleInterpolationWithSpeed(pNames, right1(), 0.2)
def diveRight(): config.stiffnessOn() motionProxy.setFallManagerEnabled(False) motionProxy.angleInterpolationWithSpeed(pNames, goaliePosition(), 0.1) motionProxy.angleInterpolationWithSpeed(pNames, goalieSit(), .5) motionProxy.angleInterpolationWithSpeed(pNames, diveRight1(), .1) motionProxy.setFallManagerEnabled(True) stand.standFromSit() goaliePose()
def standFromSit(): # Stands from sit down position. config.stiffnessOn() # Turns the stiffness on. motionProxy.angleInterpolationWithSpeed("Body", sit3(), 0.3) motionProxy.angleInterpolationWithSpeed("Body", sit4(), 0.3) motionProxy.angleInterpolationWithSpeed("Body", stand6(), 0.65) motionProxy.angleInterpolationWithSpeed("Body", sit1(), 0.4) motionProxy.angleInterpolationWithSpeed("Body", sit2(), 0.2) motionProxy.angleInterpolationWithSpeed("Body", poseInt(), 1.0) fallcheck.checkFall()
def sitDown(): # Sits down from standup position. config.stiffnessOn() # Turns the stiffness on. motionProxy.angleInterpolationWithSpeed("Body", poseInt(), 1.0) motionProxy.angleInterpolationWithSpeed("Body", sit2(), 1.0) motionProxy.angleInterpolationWithSpeed("Body", sit1(), 0.4) motionProxy.angleInterpolationWithSpeed("Body", stand6(), 0.7) motionProxy.angleInterpolationWithSpeed("Body", sit4(), 0.4) motionProxy.angleInterpolationWithSpeed("Body", sit5(), 0.4) motionProxy.angleInterpolationWithSpeed("Body", sit3(), 1.0) fallcheck.checkFall()
def alignGoalie(): config.stiffnessOn() config.poseInit() #Make sure top camera is active camera.topCamera() # Start looking for red ball to track. redballtracker.stopTracker() redballtracker.startTracker() redballtracker.setWholeBodyOn(True) print "Tracking red ball" #Store initial red ball positon to variable initialredballposition = redballtracker.getPosition() print "Initial Position: ", initialredballposition cameras = 0 #Top camera count = 0 counter = 0 #Used for Redballposition times = 0 headpitchangle = 1.433 while redballtracker.isActive(): # runs while loop as long as redballtracker is active while redballtracker.getPosition() == initialredballposition: # Ball lost. if redballtracker.isNewData() == False and count == 0: # Ball still lost.true if a new Red Ball was detected since the last getPosition(). print "Looking for red ball" motionProxy.setAngles(['HeadYaw', 'HeadPitch'], [-0.5, headpitchangle], 0.07) time.sleep(3) count = 1 cameras = 0 # top camera times += 1 elif redballtracker.isNewData() == False and count == 1: motionProxy.setAngles(['HeadYaw', 'HeadPitch'], [0.5, headpitchangle], 0.07) time.sleep(3) count = 0 cameras = 1 # bottom camera times += 1 # the next if condition is to look up slightly if he can't find the ball and then when it looks up 3 times # it decides turns left and starts all over again. if times > 1: number = float(times) if number % 2 == 1: if headpitchangle < 0: headpitchangle = 1.433 walk.ultimateWalkTo(0, 0, 2) elif headpitchangle > .5: headpitchangle = .2 else: headpitchangle = headpitchangle - 0.2 print headpitchangle else: pass foundredballposition = redballtracker.getPosition() print "Found red ball position: ", foundredballposition redballtracker.stopTracker() walk.ultimatewalkto(0, foundredballposition[1], 0)
def findRedBall(trackstop=threading.Event()): #added the trackstop so that if you want to use this #in a thread you can stop it. #This code finds the red ball. # Set stiffnes ON config.stiffnessOn() #Make sure top camera is active camera.topCamera() # Start looking for red ball to track. redballtracker.stopTracker() redballtracker.startTracker() redballtracker.setWholeBodyOn(True) print "Tracking red ball" #Store initial red ball positon to variable initialredballposition = redballtracker.getPosition() print "Initial Position: ", initialredballposition count = 0 times = 0 headpitchangle = 1.433 while redballtracker.getPosition() == initialredballposition: # Ball lost. if redballtracker.isNewData() == False and count == 0: # Ball still lost.true if a new Red Ball was detected since the last getPosition(). print "Looking for red ball" motionProxy.setAngles(['HeadYaw', 'HeadPitch'], [-0.5, headpitchangle], 0.07) time.sleep(3) count = 1 times += 1 elif redballtracker.isNewData() == False and count == 1: motionProxy.setAngles(['HeadYaw', 'HeadPitch'], [0.5, headpitchangle], 0.07) time.sleep(3) count = 0 times += 1 # the next if condition is to look up slightly if he can't find the ball and then when it looks up 3 times # it decides turns left and starts all over again. if times > 1: number = float(times) if number % 2 == 1: if headpitchangle < 0: headpitchangle = 1.433 walk.ultimateWalkTo(0, 0, 2) elif headpitchangle > .5: headpitchangle = .2 else: headpitchangle = headpitchangle - 0.2 print headpitchangle else: pass if trackstop.is_set(): initialredballposition=[-1000,-1000,-1000] print "stopped at tracking" print "found red ball" redballposition=redballtracker.getPosition() redballtracker.stopTracker() return redballposition
def standUpOnFront(): # Stands up from its belly. config.stiffnessOn() # Turns the stiffness on. motionProxy.angleInterpolationWithSpeed("Body", standUpFront(), 0.5) motionProxy.angleInterpolationWithSpeed("Body", standUpFront1(), 0.5) motionProxy.angleInterpolationWithSpeed("Body", standUpFront2(), 1.0) motionProxy.angleInterpolationWithSpeed("Body", standUpFront3(), 0.1) motionProxy.angleInterpolationWithSpeed("Body", standUpFront4(), 0.1) motionProxy.angleInterpolationWithSpeed("Body", standUpFront5(), 0.1) fallcheck.checkFall() standFromSit() print "standing front"
def slowWalkTo(x, y, z): # Set NAO in stiffness On config.stiffnessOn() # This example show customization for the both foot # with all the possible gait parameters motionProxy.walkTo(x, y, z, [ ["MaxStepX", 0.007], # step of x cm in front ["MaxStepY", 0.2], # default value ["MaxStepTheta", 0.4], # default value ["MaxStepFrequency", 1.667], # low frequency ["StepHeight", 0.015], # step height of x cm ["TorsoWx", 0], # default value ["TorsoWy", -0.1] ]) # torso bend 0.1 rad in front
def standUp(): # Stand up from its back. config.stiffnessOn() # Turns the stiffness on. motionProxy.angleInterpolationWithSpeed("Body", stand(), 1.0) motionProxy.angleInterpolationWithSpeed("Body", stand1(), 1.0) motionProxy.angleInterpolationWithSpeed("Body", stand2(), 1.0) motionProxy.angleInterpolationWithSpeed("Body", stand3(), 0.8) motionProxy.angleInterpolationWithSpeed("Body", stand4(), 0.8) motionProxy.angleInterpolationWithSpeed("Body", stand5(), 0.8) motionProxy.angleInterpolationWithSpeed("Body", stand6(), 0.8) motionProxy.angleInterpolationWithSpeed("Body", stand7(), 0.8) motionProxy.angleInterpolationWithSpeed("Body", stand8(), 0.8) motionProxy.angleInterpolationWithSpeed("Body", stand9(), 0.8) motionProxy.angleInterpolationWithSpeed("Body", poseInt(), 0.8) fallcheck.checkFall() print "standing back"
def main(): def menu(): #print what options you have print "\n*** OPTIONS ***" print "0) Quit 5) Right Kick 10) Walk Left 15) Stand Up from Sit 20) R to S 25)Goalie" print "1) Stiffness ON 6) Walk Foward 11) Walk Right 16) Find Red Ball L to R 21) R to L 26) Map" print "2) Stiffness OFF 7) Walk Backwards 12) stand up on back 17) Find Red Ball L to S 22) Stop Tracker 27) Kalman Filter" print "3) Goalie Pose 8) Turn Left 13) stand up on front 18) Find Red Ball L to L 23) Dive Left 28) Goalie intercepts clockwise" print "4) Left Kick 9) Turn Right 14) Sit Down 19) Find Red Ball R to L 24) Dive Right 29) Goalie intercepts counterclockwise" print "type what you want him to say in quotations" return input ("\nChoose your option(s) as list: ") # NOW THE PROGRAM REALLY STARTS, AS CODE IS RUN loop = 1 choice = 0 while loop == 1: choice = menu() k=0 while k < len(choice): if choice[k] == 0: loop = 0 break elif choice[k] == 1: config.stiffnessOn() elif choice[k] == 2: config.stiffnessOff() elif choice[k] == 3: goalie.goaliePose() elif choice[k] == 4: kick.kickLeftFoot() elif choice[k] == 5: kick.kickRightFoot() elif choice[k] == 6: walk.ultimateWalkTo(1, 00, 00) elif choice[k] == 7: walk.ultimateWalkTo(-1, 00, 00) elif choice[k] == 8: walk.ultimateWalkTo(0,0,2) elif choice[k] == 9: walk.ultimateWalkTo(0,0,-2) elif choice[k] == 10: walk.ultimateWalkTo(0, -1, 0) elif choice[k] == 11: walk.ultimateWalkTo(0, 1, 0) elif choice[k] == 12: stand.standUp() elif choice[k] == 13: stand.standUpOnFront() elif choice[k] == 14: stand.sitDown() elif choice[k] == 15: stand.standFromSit() elif choice[k] == 16: track.findRedBallAndKick(1) elif choice[k] == 17: track.findRedBallAndKick(2) elif choice[k] == 18: track.findRedBallAndKick(3) elif choice[k] == 19: track.findRedBallAndKick(4) elif choice[k] == 20: track.findRedBallAndKick(5) elif choice[k] == 21: track.findRedBallAndKick(6) elif choice[k] == 22: track.stopTracker() elif choice[k] == 23: dive.diveLeft() elif choice[k] == 24: dive.diveRight() elif choice[k] == 25: goalie.goalie() elif choice[k] == 26: mapping.map() elif choice[k] == 27: kalmanthreaded.kalman().start() elif choice[k] == 28: goalie.goalieImproved(1) elif choice[k] == 29: goalie.goalieImproved(0) elif choice[k] == 30: kalmanthreaded.stop() elif isinstance(choice[k], basestring): #this checks to see if the input is a string. texttospeechProxy.say(choice[k]) #if it is a string it will make the nao speak it. else: #this else statement is used to try out new files that you dont want in the comand menu. #just type out what you want it to run. #example [kalmanthreaded.get('xposition')] choice[k] k = k+1
def map(): config.stiffnessOn() config.poseInit() redballposition = track.findRedBall() mapLocation(localization.getNaoLocation(),(redballposition[0],redballposition[1]))
def goaliePose(): #This is the pose the goalie will be using. config.stiffnessOn() motionProxy.angleInterpolationWithSpeed(pNames, dive.goaliePosition(), .5)
def findRedBallAndKick(option): # Set stiffnes ON config.stiffnessOn() #Make sure top camera is active camera.topCamera() # Start looking for red ball to track. redballtracker.startTracker() redballtracker.setWholeBodyOn(True) print "Tracking red ball" #Store initial red ball positon to variable initialredballposition = redballtracker.getPosition() print "Initial Position: ", initialredballposition cameras = 0 #Top camera count = 0 counter = 0 #Used for Redballposition times = 0 headpitchangle = 1.433 while redballtracker.isActive(): # runs while loop as long as redballtracker is active while redballtracker.getPosition() == initialredballposition: # Ball lost. if redballtracker.isNewData() == False and count == 0: # Ball still lost.true if a new Red Ball was detected since the last getPosition(). print "Looking for red ball" motionProxy.setAngles(['HeadYaw', 'HeadPitch'], [-0.5, headpitchangle], 0.07) time.sleep(3) count = 1 cameras = 0 # top camera times += 1 elif redballtracker.isNewData() == False and count == 1: motionProxy.setAngles(['HeadYaw', 'HeadPitch'], [0.5, headpitchangle], 0.07) time.sleep(3) count = 0 cameras = 1 # bottom camera times += 1 # the next if condition is to look up slightly if he can't find the ball and then when it looks up 3 times # it decides turns left and starts all over again. if times > 1: number = float(times) if number % 2 == 1: if headpitchangle < 0: headpitchangle = 1.433 walk.ultimateWalkTo(0, 0, 2) elif headpitchangle > .5: headpitchangle = .2 else: headpitchangle = headpitchangle - 0.2 print headpitchangle else: pass # Ball found. # Store found red ball positon to variable. #foundredballposition and foundredballposition 2 are to make sure there are no #random jumps so the robot doesn't randomly pick a different thing to go to. if counter == 0: foundredballposition2 = redballtracker.getPosition() walk.ewalk() foundredballposition = foundredballposition2 foundredballposition2 = redballtracker.getPosition() print "Found red ball position: ", foundredballposition2 counter = 1 if redballtracker.isNewData() == False and count > 1000 or foundredballposition[0]/foundredballposition2[0] > 2: #means it lost the ball walk.stop() #start fresh initialredballposition = redballtracker.getPosition() count = 0 counter = 0 elif cameras == 0 and foundredballposition2[0] < .57 and foundredballposition[0]/foundredballposition2[0] <= 2: walk.stop() print "I made it" walk.ultimateWalkTo(0.28, foundredballposition2[1]/1.5, foundredballposition2[2]/2.1) redballtracker.stopTracker() print "I made it" if option == 1: walk.ultimateWalkTo(0, -.01, 0) kick.kickLeftFoot() elif option == 2: walk.ultimateWalkTo(0, -.075, 0) kick.kickLeftFoot() elif option == 3: walk.ultimateWalkTo(0, -.14, 0) kick.kickLeftFoot() if option == 4: walk.ultimateWalkTo(0, .01, 0) kick.kickRightFoot() elif option == 5: walk.ultimateWalkTo(0, .075, 0) kick.kickRightFoot() elif option == 6: walk.ultimateWalkTo(0, .14, 0) kick.kickRightFoot() config.poseInit() elif cameras == 1 and foundredballposition2[0] < .4 and foundredballposition[0]/foundredballposition2[0] <= 2: walk.stop() print "I made it, bottom camera" kick.kickRightFoot() redballtracker.stopTracker() config.poseInit() elif redballtracker.isNewData() == True and foundredballposition2[0] >= .4: count = 2 elif redballtracker.isNewData() == False and count <= 1000: count += 1 print count
def goaliePose(): config.stiffnessOn() motionProxy.angleInterpolationWithSpeed(pNames, goaliePosition(), 0.3)