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)
