def avoidRight(nav):
if nav.firstFrame():
avoidDest = RelRobotLocation(0, -25, 0)
helper.setOdometryDestination(nav, avoidDest)
return nav.stay()
return Transition.getNextState(nav, avoidRight)
def dodge(nav):
# return
#I'm making an executive decision and TURNING OFF DODGING
if nav.firstFrame():
nav.dodging = True
if nav.brain.ball.vis.frames_off < 5:
nav.brain.tracker.trackBall()
else:
nav.brain.tracker.trackObstacle(dodge.obstaclePosition)
## SET UP the dodge direction based on where the obstacle is
# if directly in front of us, move back and to one side based on
# where the goToPosition dest is
dodge.speed = Navigator.BRISK_SPEED
obstacleInfo = constants.OBS_DICT[dodge.obstaclePosition]
helper.createAndSendWalkVector(nav, dodge.speed * obstacleInfo[0],
dodge.speed * obstacleInfo[1], 0)
print "Dodging ", obstacleInfo[2], " Obstacle"
if navTrans.doneDodging(nav):
nav.dodging = False
nav.brain.tracker.repeatWideSnapPan()
return nav.goLater('briefStand')
return nav.stay()
def walkingTo(nav):
"""
State to be used for odometry walking.
"""
if nav.firstFrame():
nav.brain.interface.motionRequest.reset_odometry = True
nav.brain.interface.motionRequest.timestamp = int(nav.brain.time *
1000)
helper.stand(nav)
return nav.stay()
# TODO why check standing?
if nav.brain.interface.motionStatus.standing:
walkingTo.currentOdo = RelRobotLocation(nav.brain.interface.odometry.x,
nav.brain.interface.odometry.y,
nav.brain.interface.odometry.h)
if len(walkingTo.destQueue) > 0:
dest = walkingTo.destQueue.popleft()
helper.setOdometryDestination(nav, dest, walkingTo.speed)
elif locationsMatch(nav.destination, walkingTo.currentOdo):
return nav.goNow('standing')
return nav.stay()
def walking(nav):
"""
State to be used for velocity walking.
"""
helper.setSpeed(nav, walking.speeds)
return Transition.getNextState(nav, walking)
def walking(nav):
"""
State to be used for velocity walking.
"""
helper.setSpeed(nav, walking.speeds)
return Transition.getNextState(nav, walking)
def dodge(nav):
if nav.firstFrame():
nav.dodging = True
if nav.brain.ball.vis.frames_off < 5:
nav.brain.tracker.trackBall()
else:
nav.brain.tracker.trackObstacle(dodge.obstaclePosition)
## SET UP the dodge direction based on where the obstacle is
# if directly in front of us, move back and to one side based on
# where the goToPosition dest is
dodge.speed = Navigator.BRISK_SPEED
obstacleInfo = constants.OBS_DICT[dodge.obstaclePosition]
helper.createAndSendWalkVector(nav,
dodge.speed*obstacleInfo[0],
dodge.speed*obstacleInfo[1],
0)
print "Dodging ", obstacleInfo[2], " Obstacle"
if navTrans.doneDodging(nav):
nav.dodging = False
nav.brain.tracker.repeatBasicPan()
return nav.goLater('briefStand')
return nav.stay()
def avoidRightObstacle(nav):
"""
dodges left if we only detect something to the left of us
"""
if nav.firstFrame():
nav.doneAvoidingCounter = 0
nav.printf(nav.brain.sonar)
nav.printf("Avoid by left dodge");
helper.setSpeed(nav, (0, constants.DODGE_LEFT_SPEED, 0))
avoidLeft = navTrans.shouldAvoidObstacleLeft(nav)
avoidRight = navTrans.shouldAvoidObstacleRight(nav)
if (avoidLeft and avoidRight):
return nav.goLater('avoidFrontObstacle')
elif avoidLeft:
return nav.goLater('avoidLeftObstacle')
elif avoidRight:
nav.doneAvoidingCounter -= 1
nav.doneAvoidingCounter = max(0, nav.doneAvoidingCounter)
return nav.stay()
else:
nav.doneAvoidingCounter += 1
if nav.doneAvoidingCounter > constants.DONE_AVOIDING_FRAMES_THRESH:
nav.shouldAvoidObstacleRight = 0
nav.shouldAvoidObstacleLeft = 0
return nav.goLater(nav.preAvoidState)
return nav.stay()
def goToPosition(nav):
"""
Go to a position set in the navigator. General go to state. Goes
towards a location on the field stored in dest.
The location can be a RobotLocation, Location, RelRobotLocation, RelLocation
Absolute locations get transformed to relative locations based on current loc
For relative locations we use our bearing to that point as the heading
"""
relDest = helper.getRelativeDestination(nav.brain.loc, goToPosition.dest)
goToPosition.deltaDest = relDest # cache it for later use
# if nav.counter % 10 is 0:
# print "going to " + str(relDest)
# print "ball is at {0}, {1}, {2} ".format(nav.brain.ball.loc.relX,
# nav.brain.ball.loc.relY,
# nav.brain.ball.loc.bearing)
if goToPosition.adaptive and relDest.relX >= 0:
#reduce the speed if we're close to the target
speed = helper.adaptSpeed(relDest.dist, constants.ADAPT_DISTANCE,
goToPosition.speed)
else:
speed = goToPosition.speed
# print "distance {0} and speed {1}".format(relDest.dist, speed)
#if y-distance is small, ignore it to avoid strafing
#strafelessDest = helper.getStrafelessDest(relDest)
helper.setDestination(nav, relDest, speed)
# if navTrans.shouldAvoidObstacle(nav):
# return nav.goLater('avoidObstacle')
return Transition.getNextState(nav, goToPosition)
def avoidRight(nav):
if nav.firstFrame():
avoidDest = RelRobotLocation(0, -25, 0)
helper.setOdometryDestination(nav, avoidDest)
return nav.stay()
return Transition.getNextState(nav, avoidRight)
def goToPosition(nav):
"""
Go to a position set in the navigator. General go to state. Goes
towards a location on the field stored in dest.
The location can be a RobotLocation, Location, RelRobotLocation, RelLocation
Absolute locations get transformed to relative locations based on current loc
For relative locations we use our bearing to that point as the heading
"""
relDest = helper.getRelativeDestination(nav.brain.my, goToPosition.dest)
goToPosition.deltaDest = relDest # cache it for later use
# if nav.counter % 10 is 0:
# print "going to " + str(relDest)
# print "ball is at {0}, {1}, {2} ".format(nav.brain.ball.loc.relX,
# nav.brain.ball.loc.relY,
# nav.brain.ball.loc.bearing)
if goToPosition.adaptive and relDest.relX >= 0:
#reduce the speed if we're close to the target
speed = helper.adaptSpeed(relDest.dist,
constants.ADAPT_DISTANCE,
goToPosition.speed)
else:
speed = goToPosition.speed
# print "distance {0} and speed {1}".format(relDest.dist, speed)
#if y-distance is small, ignore it to avoid strafing
#strafelessDest = helper.getStrafelessDest(relDest)
helper.setDestination(nav, relDest, speed)
# if navTrans.shouldAvoidObstacle(nav):
# return nav.goLater('avoidObstacle')
return Transition.getNextState(nav, goToPosition)
def briefStand(nav):
if nav.firstFrame():
helper.stand(nav)
if nav.counter > 3:
return nav.goLater('goToPosition')
return nav.stay()
def briefStand(nav):
if nav.firstFrame():
helper.stand(nav)
if nav.counter > 3:
return nav.goLater('goToPosition')
return nav.stay()
def atPosition(nav):
"""
Switches back if we're not at the destination anymore.
"""
if nav.firstFrame():
helper.stand(nav)
return Transition.getNextState(nav, atPosition)
def atPosition(nav):
"""
Switches back if we're not at the destination anymore.
"""
if nav.firstFrame():
helper.stand(nav)
return Transition.getNextState(nav, atPosition)
def walkInPlace(nav):
if nav.firstFrame():
helper.walkInPlace(nav)
# if nav.counter > 20:
# return nav.goLater('atPosition')
return nav.stay()
def walkInPlace(nav):
if nav.firstFrame():
helper.walkInPlace(nav)
# if nav.counter > 20:
# return nav.goLater('atPosition')
return nav.stay()
def scriptedMove(nav):
'''State that we stay in while doing sweet moves'''
if nav.firstFrame():
helper.executeMove(nav.brain.motion, scriptedMove.sweetMove)
return nav.stay()
if not nav.brain.motion.isBodyActive():
return nav.goNow('stopped')
return nav.stay()
def walking(nav):
"""
State to be used for velocity walking.
"""
helper.setSpeed(nav, walking.speeds)
if navTrans.shouldDodge(nav):
return nav.goNow('dodge')
return Transition.getNextState(nav, walking)
def kickEngine(nav):
"""
State that we stay in while calling kick engine
"""
if nav.firstFrame():
helper.executeKickEngine(nav, kickEngine.kickType)
return nav.stay()
helper.stand(nav)
return nav.stay()
def kickEngine(nav):
"""
State that we stay in while calling kick engine
"""
if nav.firstFrame():
helper.executeKickEngine(nav, kickEngine.kickType)
return nav.stay()
helper.stand(nav)
return nav.stay()
def walking(nav):
"""
State to be used for velocity walking.
"""
if (walking.speeds != walking.lastSpeeds) or not nav.brain.interface.motionStatus.walk_is_active:
helper.setSpeed(nav, walking.speeds)
walking.lastSpeeds = walking.speeds
return Transition.getNextState(nav, walking)
def walking(nav):
"""
State to be used when setSpeed is called
"""
if (walking.speeds != walking.lastSpeeds) or not nav.brain.motion.isWalkActive():
helper.setSpeed(nav, walking.speeds)
walking.lastSpeeds = walking.speeds
return Transition.getNextState(nav, walking)
def scriptedMove(nav):
"""State that we stay in while doing sweet moves"""
if nav.firstFrame():
helper.executeMove(nav, scriptedMove.sweetMove)
return nav.stay()
if not nav.brain.interface.motionStatus.body_is_active:
return nav.goNow("stopped")
return nav.stay()
def scriptedMove(nav):
'''State that we stay in while doing sweet moves'''
if nav.firstFrame():
helper.executeMove(nav, scriptedMove.sweetMove)
return nav.stay()
if not nav.brain.interface.motionStatus.body_is_active:
return nav.goNow('stopped')
return nav.stay()
def walking(nav):
"""
State to be used for velocity walking.
"""
helper.setSpeed(nav, walking.speeds)
if navTrans.shouldDodge(nav):
return nav.goNow('dodge')
return Transition.getNextState(nav, walking)
def scriptedMove(nav):
'''State that we stay in while doing sweet moves'''
if nav.firstFrame():
helper.executeMove(nav.brain.motion, scriptedMove.sweetMove)
return nav.stay()
if not nav.brain.motion.isBodyActive():
return nav.goNow('stopped')
return nav.stay()
def walking(nav):
"""
State to be used when setSpeed is called
"""
if ((walking.speeds != walking.lastSpeeds)
or not nav.brain.interface.motionStatus.walk_is_active):
helper.setSpeed(nav, walking.speeds)
walking.lastSpeeds = walking.speeds
return Transition.getNextState(nav, walking)
def walkingAndKicking(nav):
"""
State to be used for velocity walking AND motion kicking.
"""
if ((walking.speeds != walking.lastSpeeds)
or not nav.brain.interface.motionStatus.walk_is_active):
helper.createAndSendMotionKickVector(nav, *walking.speeds)
walking.lastSpeeds = walking.speeds
return Transition.getNextState(nav, walking)
def atPosition(nav):
"""
Switches back if we're not at the destination anymore.
"""
if nav.firstFrame():
# print("In at position, walking in place")
helper.walkInPlace(nav)
elif nav.counter == 5:
helper.stand(nav)
return Transition.getNextState(nav, atPosition)
def walkingAndKicking(nav):
"""
State to be used for velocity walking AND motion kicking.
"""
if ((walking.speeds != walking.lastSpeeds)
or not nav.brain.interface.motionStatus.walk_is_active):
helper.createAndSendMotionKickVector(nav, *walking.speeds)
walking.lastSpeeds = walking.speeds
return Transition.getNextState(nav, walking)
def atPosition(nav):
"""
Switches back if we're not at the destination anymore.
"""
if nav.firstFrame():
# print("In at position, walking in place")
helper.walkInPlace(nav)
elif nav.counter == 5:
helper.stand(nav)
return Transition.getNextState(nav, atPosition)
def walking(nav):
"""
State to be used when setSpeed is called
"""
if ((walking.speeds != walking.lastSpeeds)
or not nav.brain.interface.motionStatus.walk_is_active):
helper.setSpeed(nav, walking.speeds)
walking.lastSpeeds = walking.speeds
return Transition.getNextState(nav, walking)
def walking(nav):
"""
State to be used when setSpeed is called
"""
if (walking.speeds !=
walking.lastSpeeds) or not nav.brain.motion.isWalkActive():
helper.setSpeed(nav, walking.speeds)
walking.lastSpeeds = walking.speeds
return Transition.getNextState(nav, walking)
def stop(nav):
"""
Wait until the walk is finished.
"""
if nav.firstFrame():
# stop walk vectors
helper.stand(nav)
if not nav.brain.motion.isWalkActive():
return nav.goNow('stopped')
return nav.stay()
def dodge(nav):
# TODO: HACK FOR BRAZIL - THIS IS USED WHEN IT'S ARM-ONLY DETECTION
if nav.firstFrame():
## SET UP the dodge direction based on where the obstacle is
# if directly in front of us, move back and to one side based on
# where the goToPosition dest is
if dodge.armPosition == 1:
print "Dodging NORTH obstacle"
relDest = helper.getRelativeDestination(nav.brain.loc,
goToPosition.dest)
if relDest.relY <= 0:
direction = -1
else:
direction = 1
dodge.dest = RelRobotLocation(-15, direction * 10, 0)
elif dodge.armPosition == 2:
print "Dodging NORTHEAST obstacle"
dodge.dest = RelRobotLocation(-5, 15, 0)
elif dodge.armPosition == 3:
print "Dodging EAST obstacle"
dodge.dest = RelRobotLocation(0, 20, 0)
elif dodge.armPosition == 4:
print "Dodging SOUTHEAST obstacle"
dodge.dest = RelRobotLocation(5, 15, 0)
# if directly behind us, move forward and to one side based on
# where the goToPosition dest is
elif dodge.armPosition == 5:
print "Dodging SOUTH obstacle"
relDest = helper.getRelativeDestination(nav.brain.loc,
goToPosition.dest)
if relDest.relY <= 0:
direction = -1
else:
direction = 1
dodge.dest = RelRobotLocation(15, direction * 10, 0)
elif dodge.armPosition == 6:
print "Dodging SOUTHWEST obstacle"
dodge.dest = RelRobotLocation(5, -15, 0)
elif dodge.armPosition == 7:
print "Dodging WEST obstacle"
dodge.dest = RelRobotLocation(0, -20, 0)
elif dodge.armPosition == 8:
print "Dodging NORTHWEST obstacle"
dodge.dest = RelRobotLocation(-5, -15, 0)
else:
return
dest = RelRobotLocation(dodge.dest.relX + random(),
dodge.dest.relY + random(),
dodge.dest.relH + random())
helper.setDestination(nav, dest, 0.5)
return Transition.getNextState(nav, dodge)
def dodge(nav):
# TODO: HACK FOR BRAZIL - THIS IS USED WHEN IT'S ARM-ONLY DETECTION
if nav.firstFrame():
## SET UP the dodge direction based on where the obstacle is
# if directly in front of us, move back and to one side based on
# where the goToPosition dest is
if dodge.armPosition == 1:
print "Dodging NORTH obstacle"
relDest = helper.getRelativeDestination(nav.brain.loc,
goToPosition.dest)
if relDest.relY <= 0:
direction = -1
else:
direction = 1
dodge.dest = RelRobotLocation(-15, direction*10, 0)
elif dodge.armPosition == 2:
print "Dodging NORTHEAST obstacle"
dodge.dest = RelRobotLocation(-5, 15, 0)
elif dodge.armPosition == 3:
print "Dodging EAST obstacle"
dodge.dest = RelRobotLocation(0, 20, 0)
elif dodge.armPosition == 4:
print "Dodging SOUTHEAST obstacle"
dodge.dest = RelRobotLocation(5, 15, 0)
# if directly behind us, move forward and to one side based on
# where the goToPosition dest is
elif dodge.armPosition == 5:
print "Dodging SOUTH obstacle"
relDest = helper.getRelativeDestination(nav.brain.loc,
goToPosition.dest)
if relDest.relY <= 0:
direction = -1
else:
direction = 1
dodge.dest = RelRobotLocation(15, direction*10, 0)
elif dodge.armPosition == 6:
print "Dodging SOUTHWEST obstacle"
dodge.dest = RelRobotLocation(5, -15, 0)
elif dodge.armPosition == 7:
print "Dodging WEST obstacle"
dodge.dest = RelRobotLocation(0, -20, 0)
elif dodge.armPosition == 8:
print "Dodging NORTHWEST obstacle"
dodge.dest = RelRobotLocation(-5, -15, 0)
else:
return
dest = RelRobotLocation(dodge.dest.relX + random(),
dodge.dest.relY + random(),
dodge.dest.relH + random())
helper.setDestination(nav, dest, 0.5)
return Transition.getNextState(nav, dodge)
def stand(nav):
"""
Transitional state between walking and standing
Could still be walking, but we can give it new walk commands
so we shouldn't wait to go for it to go to standing before we
give it new commands
"""
if nav.firstFrame():
helper.stand(nav)
if not nav.brain.motion.isWalkActive():
return nav.goNow('standing')
return nav.stay()
def stand(nav):
"""
Transitional state between walking and standing
Could still be walking, but we can give it new walk commands
so we shouldn't wait to go for it to go to standing before we
give it new commands
"""
if nav.firstFrame():
helper.stand(nav)
if not nav.brain.motion.isWalkActive():
return nav.goNow('standing')
return nav.stay()
def destinationWalkingTo(nav):
"""
State to be used for destination walking.
"""
if nav.firstFrame():
destinationWalkingTo.enqueAZeroVector = False
if not nav.brain.motion.calibrated:
helper.stand(nav)
return nav.stay()
if nav.counter < 4:
# print("In dest walking to, walking in place")
helper.walkInPlace(nav)
return nav.stay()
destinationWalkingTo.speed = nav.velocity
if fabs(nav.requestVelocity - nav.velocity) > Navigator.SPEED_CHANGE:
nav.velocity += copysign(Navigator.SPEED_CHANGE, (nav.requestVelocity - nav.velocity))
if len(destinationWalkingTo.destQueue) > 0:
dest = destinationWalkingTo.destQueue.popleft()
helper.setDestination(nav, dest,
destinationWalkingTo.speed,
destinationWalkingTo.kick)
# destinationWalkingTo.enqueAZeroVector = True
elif destinationWalkingTo.enqueAZeroVector:
helper.setDestination(nav, RelRobotLocation(0,0,0),
destinationWalkingTo.speed,
destinationWalkingTo.kick)
destinationWalkingTo.enqueAZeroVector = False
return nav.stay()
def destinationWalkingTo(nav):
"""
State to be used for destination walking.
"""
if nav.firstFrame():
destinationWalkingTo.enqueAZeroVector = False
if not nav.brain.motion.calibrated:
helper.stand(nav)
return nav.stay()
if nav.counter < 4:
# print("In dest walking to, walking in place")
helper.walkInPlace(nav)
return nav.stay()
destinationWalkingTo.speed = nav.velocity
if fabs(nav.requestVelocity - nav.velocity) > Navigator.SPEED_CHANGE:
nav.velocity += copysign(Navigator.SPEED_CHANGE,
(nav.requestVelocity - nav.velocity))
if len(destinationWalkingTo.destQueue) > 0:
dest = destinationWalkingTo.destQueue.popleft()
helper.setDestination(nav, dest, destinationWalkingTo.speed,
destinationWalkingTo.kick)
# destinationWalkingTo.enqueAZeroVector = True
elif destinationWalkingTo.enqueAZeroVector:
helper.setDestination(nav, RelRobotLocation(0, 0, 0),
destinationWalkingTo.speed,
destinationWalkingTo.kick)
destinationWalkingTo.enqueAZeroVector = False
return nav.stay()
def dodge(nav):
if nav.firstFrame():
## SET UP the dodge direction based on where the obstacle is
# if directly in front of us, move back and to one side based on
# where the goToPosition dest is
if dodge.position is dodge.position.NORTH:
print "Dodging NORTH obstacle"
relDest = helper.getRelativeDestination(nav.brain.loc,
goToPosition.dest)
if relDest.relY <= 0:
direction = -1
else:
direction = 1
dodgeDest = RelRobotLocation(-15, direction * 10, 0)
elif dodge.position is dodge.position.NORTHEAST:
print "Dodging NORTHEAST obstacle"
dodgeDest = RelRobotLocation(0, 15, 0)
elif dodge.position is dodge.position.EAST:
print "Dodging EAST obstacle"
dodgeDest = RelRobotLocation(0, 20, 0)
elif dodge.position is dodge.position.SOUTHEAST:
print "Dodging SOUTHEAST obstacle"
dodgeDest = RelRobotLocation(0, 15, 0)
# if directly behind us, move forward and to one side based on
# where the goToPosition dest is
elif dodge.position is dodge.position.SOUTH:
print "Dodging SOUTH obstacle"
relDest = helper.getRelativeDestination(nav.brain.loc,
goToPosition.dest)
if relDest.relY <= 0:
direction = -1
else:
direction = 1
dodgeDest = RelRobotLocation(15, direction * 10, 0)
elif dodge.position is dodge.position.SOUTHWEST:
print "Dodging SOUTHWEST obstacle"
dodgeDest = RelRobotLocation(0, -15, 0)
elif dodge.position is dodge.position.WEST:
print "Dodging WEST obstacle"
dodgeDest = RelRobotLocation(0, -20, 0)
elif dodge.position is dodge.position.NORTHWEST:
print "Dodging NORTHWEST obstacle"
dodgeDest = RelRobotLocation(0, -15, 0)
helper.setOdometryDestination(nav, dodgeDest)
return Transition.getNextState(nav, dodge)
def dodge(nav):
if nav.firstFrame():
## SET UP the dodge direction based on where the obstacle is
# if directly in front of us, move back and to one side based on
# where the goToPosition dest is
if dodge.position is dodge.position.NORTH:
print "Dodging NORTH obstacle"
relDest = helper.getRelativeDestination(nav.brain.loc,
goToPosition.dest)
if relDest.relY <= 0:
direction = -1
else:
direction = 1
dodgeDest = RelRobotLocation(-15, direction*10, 0)
elif dodge.position is dodge.position.NORTHEAST:
print "Dodging NORTHEAST obstacle"
dodgeDest = RelRobotLocation(0, 15, 0)
elif dodge.position is dodge.position.EAST:
print "Dodging EAST obstacle"
dodgeDest = RelRobotLocation(0, 20, 0)
elif dodge.position is dodge.position.SOUTHEAST:
print "Dodging SOUTHEAST obstacle"
dodgeDest = RelRobotLocation(0, 15, 0)
# if directly behind us, move forward and to one side based on
# where the goToPosition dest is
elif dodge.position is dodge.position.SOUTH:
print "Dodging SOUTH obstacle"
relDest = helper.getRelativeDestination(nav.brain.loc,
goToPosition.dest)
if relDest.relY <= 0:
direction = -1
else:
direction = 1
dodgeDest = RelRobotLocation(15, direction*10, 0)
elif dodge.position is dodge.position.SOUTHWEST:
print "Dodging SOUTHWEST obstacle"
dodgeDest = RelRobotLocation(0, -15, 0)
elif dodge.position is dodge.position.WEST:
print "Dodging WEST obstacle"
dodgeDest = RelRobotLocation(0, -20, 0)
elif dodge.position is dodge.position.NORTHWEST:
print "Dodging NORTHWEST obstacle"
dodgeDest = RelRobotLocation(0, -15, 0)
helper.setOdometryDestination(nav, dodgeDest)
return Transition.getNextState(nav, dodge)
def walkingTo(nav):
"""
State to be used for odometry walking.
"""
if nav.firstFrame():
helper.stand(nav)
return nav.stay()
if nav.brain.interface.motionStatus.standing:
if len(walkingTo.destQueue) > 0:
dest = walkingTo.destQueue.popleft()
helper.setOdometryDestination(nav, dest, walkingTo.speed)
return nav.stay()
else:
return nav.goNow("standing")
return nav.stay()
def walkingTo(nav):
"""
State to be used for odometry walking.
"""
if nav.firstFrame():
helper.stand(nav)
return nav.stay()
if nav.brain.interface.motionStatus.standing:
if len(walkingTo.destQueue) > 0:
dest = walkingTo.destQueue.popleft()
helper.setOdometryDestination(nav, dest, walkingTo.speed)
return nav.stay()
else:
return nav.goNow('standing')
return nav.stay()
def walkingTo(nav):
"""
Walks to a relative location based on odometry
"""
if nav.firstFrame():
helper.stand(nav)
return nav.stay()
if nav.brain.interface.motionStatus.standing:
if len(walkingTo.destQueue) > 0:
dest = walkingTo.destQueue.popleft()
helper.setOdometryDestination(nav, dest, walkingTo.speed)
return nav.stay()
else:
return nav.goNow('standing')
return nav.stay()
def walkingTo(nav):
"""
Walks to a relative location based on odometry
"""
if nav.firstFrame():
helper.stand(nav)
return nav.stay()
if nav.brain.motion.isStanding():
if len(walkingTo.destQueue) > 0:
dest = walkingTo.destQueue.popleft()
helper.setOdometryDestination(nav, dest, walkingTo.speed)
return nav.stay()
else:
return nav.goNow('standing')
return nav.stay()
def stand(nav):
"""
Transitional state between walking and standing
So we can give new walk commands before we complete
the stand if desired
"""
if nav.firstFrame():
helper.stand(nav)
return nav.stay()
if (nav.counter % 300 == 0):
helper.stand(nav)
return nav.stay()
if not nav.brain.interface.motionStatus.walk_is_active:
return nav.goNow('standing')
return nav.stay()
def stand(nav):
"""
Transitional state between walking and standing
So we can give new walk commands before we complete
the stand if desired
"""
if nav.firstFrame():
helper.stand(nav)
return nav.stay()
if nav.counter % 300 == 0:
helper.stand(nav)
return nav.stay()
if not nav.brain.interface.motionStatus.walk_is_active:
return nav.goNow("standing")
return nav.stay()
def walkingTo(nav):
"""
State to be used for odometry walking.
"""
if nav.firstFrame():
print ("Resetting odometry!")
nav.brain.interface.motionRequest.reset_odometry = True
nav.brain.interface.motionRequest.timestamp = int(nav.brain.time * 1000)
helper.stand(nav)
return nav.stay()
walkingTo.currentOdo = RelRobotLocation(nav.brain.interface.odometry.x,
nav.brain.interface.odometry.y,
nav.brain.interface.odometry.h)
# TODO why check standing?
if nav.brain.interface.motionStatus.standing:
if len(walkingTo.destQueue) > 0:
dest = walkingTo.destQueue.popleft()
helper.setOdometryDestination(nav, dest, walkingTo.speed)
print ("MY dest: ", dest.relX, dest.relY, dest.relH)
if locationsMatch(nav.destination, walkingTo.currentOdo):
return nav.goNow('standing')
# walkingTo.currentOdow = RelRobotLocation(nav.brain.interface.odometry.x,
# nav.brain.interface.odometry.y,
# nav.brain.interface.odometry.h)
if nav.counter % 30 == 0:
print "Current odo:"
print ("x:", walkingTo.currentOdo.relX)
print ("y:", walkingTo.currentOdo.relY)
print ("h:", walkingTo.currentOdo.relH)
# print "Current odow:"
# print ("x:", walkingTo.currentOdow.relX)
# print ("y:", walkingTo.currentOdow.relY)
# print ("h:", walkingTo.currentOdow.relH)
# print "---------------"
return nav.stay()
def destinationWalkingTo(nav):
"""
State to be used for destination walking.
"""
if nav.firstFrame():
destinationWalkingTo.enqueAZeroVector = False
if len(destinationWalkingTo.destQueue) > 0:
dest = destinationWalkingTo.destQueue.popleft()
helper.setDestination(nav, dest, destinationWalkingTo.speed,
destinationWalkingTo.kick)
destinationWalkingTo.enqueAZeroVector = True
elif destinationWalkingTo.enqueAZeroVector:
helper.setDestination(nav, RelRobotLocation(0, 0, 0),
destinationWalkingTo.speed,
destinationWalkingTo.kick)
destinationWalkingTo.enqueAZeroVector = False
return nav.stay()
def dodge(nav):
order = [0, 1, -1, 2, -2, 3, -3, 4]
if nav.firstFrame():
# dodge.positions[0] is position.NONE, so direction numbers are their own index
for i in range(len(order)):
temp = getIndex(int(dodge.targetDest) + order[i])
# if there is no obstacle in this direction
if not dodge.positions[temp]:
print "DODGE TO ", dodge.DDirects[temp]
dodge.dest = RelRobotLocation(constants.DGE_DESTS[temp-1][0],
constants.DGE_DESTS[temp-1][1],
constants.DGE_DESTS[temp-1][2])
break
# TODO the worst hack I have ever written, sorry -- Josh Imhoff
dest2 = RelRobotLocation(dodge.dest.relX + random(),
dodge.dest.relY + random(),
dodge.dest.relH + random())
helper.setDestination(nav, dest2, 0.5)
return Transition.getNextState(nav, dodge)
def walkingTo(nav):
"""
Walks to a relative location based on odometry
"""
loc = nav.brain.loc
dest = walkingTo.dest
if nav.firstFrame():
#@todo: make a method that returns odometry as a tuple in PyLoc?
walkingTo.startingOdometry = (loc.lastOdoX, loc.lastOdoY, loc.lastOdoTheta)
deltaOdo = helper.getDeltaOdometry(loc, walkingTo.startingOdometry)
walkingTo.deltaDest = dest - (deltaOdo.relX, deltaOdo.relY, deltaOdo.relH)
# print "Delta dest {0}".format(walkingTo.deltaDest)
# print str(dest)
# print str(deltaOdo)
#walk the rest of the way
helper.setDestination(nav, walkingTo.deltaDest, walkingTo.speed)
return Transition.getNextState(nav, walkingTo)
def destinationWalkingTo(nav):
"""
State to be used for destination walking.
"""
if nav.firstFrame():
destinationWalkingTo.enqueAZeroVector = False
if len(destinationWalkingTo.destQueue) > 0:
dest = destinationWalkingTo.destQueue.popleft()
helper.setDestination(nav, dest,
destinationWalkingTo.speed,
destinationWalkingTo.kick)
destinationWalkingTo.enqueAZeroVector = True
elif destinationWalkingTo.enqueAZeroVector:
helper.setDestination(nav, RelRobotLocation(0,0,0),
destinationWalkingTo.speed,
destinationWalkingTo.kick)
destinationWalkingTo.enqueAZeroVector = False
return nav.stay()
def stand(nav):
"""
Transitional state between walking and standing
So we can give new walk commands before we complete
the stand if desired
"""
if (nav.brain.player.gameState == 'gameInitial'
or nav.brain.player.gameState == 'gameSet'
or nav.lastState == 'stand'):
helper.stand(nav)
return nav.stay()
if nav.counter < 10:
# print("In stand, walking in place")
helper.walkInPlace(nav)
return nav.stay()
elif nav.counter < 15:
helper.stand(nav)
# if nav.firstFrame():
# helper.stand(nav)
# return nav.stay()
# if (nav.counter % 300 == 0):
# return nav.stay()
if not nav.brain.interface.motionStatus.walk_is_active:
return nav.goNow('standing')
# helper.stand(nav)
return nav.stay()
def stand(nav):
"""
Transitional state between walking and standing
So we can give new walk commands before we complete
the stand if desired
"""
if (nav.brain.player.gameState == 'gameInitial'
or nav.brain.player.gameState == 'gameSet'
or nav.lastState == 'stand'):
helper.stand(nav)
return nav.stay()
if nav.counter < 10:
# print("In stand, walking in place")
helper.walkInPlace(nav)
return nav.stay()
elif nav.counter < 15:
helper.stand(nav)
# if nav.firstFrame():
# helper.stand(nav)
# return nav.stay()
# if (nav.counter % 300 == 0):
# return nav.stay()
if not nav.brain.interface.motionStatus.walk_is_active:
return nav.goNow('standing')
# helper.stand(nav)
return nav.stay()
def walkingTo(nav):
"""
State to be used for odometry walking.
"""
if nav.firstFrame():
print("Resetting odometry!")
nav.brain.interface.motionRequest.reset_odometry = True
nav.brain.interface.motionRequest.timestamp = int(nav.brain.time *
1000)
print("MY dest: ", nav.destination.relX, nav.destination.relY,
nav.destination.relH)
helper.stand(nav)
return nav.stay()
walkingTo.currentOdo = RelRobotLocation(nav.brain.interface.odometry.x,
nav.brain.interface.odometry.y,
nav.brain.interface.odometry.h)
# TODO why check standing?
if nav.brain.interface.motionStatus.standing:
if len(walkingTo.destQueue) > 0:
dest = walkingTo.destQueue.popleft()
# dest.relH = 0
helper.setOdometryDestination(nav, dest, walkingTo.speed)
# helper.setDestination(nav, dest, walkingTo.speed)
print("MY dest: ", dest.relX, dest.relY, dest.relH)
if locationsMatch(nav.destination, walkingTo.currentOdo):
return nav.goNow('standing')
if nav.counter % 30 == 0:
print "Current odo:"
print("x:", walkingTo.currentOdo.relX)
print("y:", walkingTo.currentOdo.relY)
print("h:", walkingTo.currentOdo.relH)
return nav.stay()
def destinationWalkingTo(nav):
"""
State to be used for destination walking.
"""
if nav.firstFrame():
destinationWalkingTo.enqueAZeroVector = False
destinationWalkingTo.speed = nav.velocity
if fabs(nav.requestVelocity - nav.velocity) > Navigator.SPEED_CHANGE:
nav.velocity += copysign(Navigator.SPEED_CHANGE,
(nav.requestVelocity - nav.velocity))
if len(destinationWalkingTo.destQueue) > 0:
dest = destinationWalkingTo.destQueue.popleft()
helper.setDestination(nav, dest, destinationWalkingTo.speed,
destinationWalkingTo.kick)
destinationWalkingTo.enqueAZeroVector = True
elif destinationWalkingTo.enqueAZeroVector:
helper.setDestination(nav, RelRobotLocation(0, 0, 0),
destinationWalkingTo.speed,
destinationWalkingTo.kick)
destinationWalkingTo.enqueAZeroVector = False
return nav.stay()
def atPosition(nav):
if nav.firstFrame():
helper.stand(nav)
return Transition.getNextState(nav, atPosition)
def walkInPlace(nav):
if nav.firstFrame():
helper.walkInPlace(nav)
return nav.stay()
def goToPosition(nav):
"""
Go to a position set in the navigator. General go to state. Goes
towards a location on the field stored in dest.
The location can be a RobotLocation, Location, RelRobotLocation, RelLocation
Absolute locations get transformed to relative locations based on current loc
For relative locations we use our bearing to that point as the heading
"""
relDest = helper.getRelativeDestination(nav.brain.loc, goToPosition.dest)
if nav.firstFrame():
# print("Resetting at position transition!!")
nav.atLocPositionTransition.reset()
if not nav.brain.motion.calibrated:
helper.stand(nav)
return nav.stay()
# if nav.counter % 10 is 0:
# print "\ngoing to " + str(relDest)
# print "ball is at {0}, {1}, {2} ".format(nav.brain.ball.loc.relX,
# nav.brain.ball.loc.relY,
# nav.brain.ball.loc.bearing)
if nav.counter < 5:
# print("In go to position, walking in place")
helper.walkInPlace(nav)
return nav.stay()
goToPosition.speed = nav.velocity
if fabs(nav.requestVelocity - nav.velocity) > Navigator.SPEED_CHANGE:
nav.velocity += copysign(Navigator.SPEED_CHANGE,
(nav.requestVelocity - nav.velocity))
if goToPosition.pb and isinstance(goToPosition.dest, RelRobotLocation):
# Calc dist to dest
dist = helper.getDistToDest(nav.brain.loc, goToPosition.dest)
if goToPosition.fast and dist < 140:
goToPosition.fast = False
goToPosition.dest = nav.brain.play.getPosition()
elif not goToPosition.fast and dist > 160:
goToPosition.fast = True
goToPosition.dest = nav.brain.play.getPositionCoord()
if isinstance(goToPosition.dest, RobotLocation):
dist = helper.getDistToDest(nav.brain.loc, goToPosition.dest)
# print("Distance: ", dist)
if dist < 30:
# print("I'm close enough ! I should not go fast anymore")
goToPosition.fast = False
goToPosition.speeds = (0.1, 0.1, 0.1)
# print("My reldest: ", str(relDest))
if goToPosition.fast:
# print("goToPosition fast")
# So that fast mode works for objects of type RobotLocation also
if isinstance(goToPosition.dest,
RobotLocation) and not goToPosition.close:
# print("It is an instance of a robot location")
fieldDest = RobotLocation(goToPosition.dest.x, goToPosition.dest.y,
0)
relDest = nav.brain.loc.relativeRobotLocationOf(fieldDest)
relDest.relH = nav.brain.loc.getRelativeBearing(fieldDest)
elif isinstance(goToPosition.dest, RelRobotLocation):
relDest = goToPosition.dest
HEADING_ADAPT_CUTOFF = 103
DISTANCE_ADAPT_CUTOFF = 10
MAX_TURN = .5
BOOK_IT_TURN_THRESHOLD = 23
BOOK_IT_DISTANCE_THRESHOLD = 50
if relDest.relH >= HEADING_ADAPT_CUTOFF:
velH = MAX_TURN
elif relDest.relH <= -HEADING_ADAPT_CUTOFF:
velH = -MAX_TURN
else:
velH = helper.adaptSpeed(relDest.relH, HEADING_ADAPT_CUTOFF,
MAX_TURN)
if relDest.relX >= DISTANCE_ADAPT_CUTOFF:
velX = goToPosition.speed
elif relDest.relX <= -DISTANCE_ADAPT_CUTOFF:
velX = -goToPosition.speed
else:
velX = helper.adaptSpeed(relDest.relX, DISTANCE_ADAPT_CUTOFF,
goToPosition.speed)
if relDest.relY >= DISTANCE_ADAPT_CUTOFF:
velY = goToPosition.speed
elif relDest.relY <= -DISTANCE_ADAPT_CUTOFF:
velY = -goToPosition.speed
else:
velY = helper.adaptSpeed(relDest.relY, DISTANCE_ADAPT_CUTOFF,
goToPosition.speed)
if fabs(relDest.dist) > BOOK_IT_DISTANCE_THRESHOLD:
goToPosition.close = False
if fabs(relDest.relH) > BOOK_IT_TURN_THRESHOLD:
if relDest.relH > 0: velH = MAX_TURN
if relDest.relH < 0: velH = -MAX_TURN
velX = 0
velY = 0
goToPosition.bookingIt = False
else:
velY = 0
goToPosition.bookingIt = True
else:
goToPosition.close = True
# TODO nikki walk unsw hack
# if relDest.relY < DISTANCE_ADAPT_CUTOFF:
# velY = 0.0
if (fabs(relDest.relH) > 20.0):
goToPosition.speeds = (0, 0, velH)
else:
goToPosition.speeds = (velX, velY, velH)
# print(" NAV: My speeds:", velX, velY, velH)
helper.setSpeed(nav, goToPosition.speeds)
else:
# print("Was not fase!")
if goToPosition.adaptive:
#reduce the speed if we're close to the target
speed = helper.adaptSpeed(relDest.dist, constants.ADAPT_DISTANCE,
goToPosition.speed)
else:
speed = goToPosition.speed
helper.setDestination(nav, relDest, speed)
# print(" NAV Setting dest: ", str(relDest))
# if navTrans.shouldDodge(nav):
# return nav.goNow('dodge')
return Transition.getNextState(nav, goToPosition)
