def saGoToTargetFacingHeading(targetX, targetY, targetH, \
maxSpeed = Action.MAX_FORWARD, maxTurn = Action.MAX_TURN):
# Variables relative to self localisation.
selfPos = Global.selfLoc.getPos()
selfH = Global.selfLoc.getHeading()
cosSelf = math.cos(math.radians(selfH))
sinSelf = math.sin(math.radians(selfH))
relX = targetX - selfPos[0]
relY = targetY - selfPos[1]
relH = hMath.normalizeAngle_180(targetH - selfH)
# now take into account that as we turn, the position of our head will change
relX += Constant.DOG_LENGTH / 2.0 * (cosSelf -
math.cos(math.radians(targetH)))
relY += Constant.DOG_LENGTH / 2.0 * (sinSelf -
math.sin(math.radians(targetH)))
relD = hMath.getLength((relX, relY))
# Calculate approximate coordinates of all the dog's legs
legs = []
for legNum in range(4):
legPos = list(selfPos)
if legNum >= 2:
legPos[0] = legPos[0] - DOG_RECT_LENGTH * cosSelf
legPos[1] = legPos[1] - DOG_RECT_LENGTH * sinSelf
if (legNum % 2) == 0:
legPos[0] = legPos[0] - DOG_RECT_WIDTH * sinSelf
legPos[1] = legPos[1] + DOG_RECT_WIDTH * cosSelf
else:
legPos[0] = legPos[0] + DOG_RECT_WIDTH * sinSelf
legPos[1] = legPos[1] - DOG_RECT_WIDTH * cosSelf
legs.append(tuple(legPos))
# If a leg coordinate lies within the rear wall, clip the Y movement
clipMove = False
for legPos in legs:
if abs(legPos[0] - Constant.FIELD_WIDTH / 2) > Constant.GOAL_WIDTH and\
legPos[1] < 2:
clipMove = True
if clipMove and (relY < 0):
relY = 0
if relX == 0 and relY == 0:
#on the dog, math.atan2(0,0) give "Value Error: math domain error"
relTheta = 0
else:
relTheta = hMath.normalizeAngle_180(
hMath.RAD2DEG(math.atan2(relY, relX)) - selfH)
forward = hMath.CLIP(relD, maxSpeed) * math.cos(hMath.DEG2RAD(relTheta))
left = hMath.CLIP(relD, maxSpeed) * math.sin(hMath.DEG2RAD(relTheta))
turnCCW = hMath.CLIP(relH, maxTurn)
Action.walk(forward, left, turnCCW, minorWalkType=Action.SkeFastForwardMWT)
def SmartSetBeacon(panLimit=90, distLimit=550):
global localiseBeacon
# Get shared variables from elsewhere.
selfLoc = Global.selfLoc
cov = VisionLink.getGPSSelfCovariance()
# Calculate the direction of the larger axis of the self covariance
b = -(cov[0][0] + cov[1][1])
c = cov[0][0] * cov[1][1] - cov[1][0] * cov[1][0]
det = b * b - 4 * c
if (det < 0.00001):
sol = -b / 2
else:
sol = (-b + math.sqrt(det)) / 2
if abs(cov[0][0] - sol) < 0.01 and cov[0][1] < 0.01:
# If self position variance is circular, all beacons are equally good
# to localise off, and so choose the one closest to current heading.
if abs(cov[1][1] - sol) < 0.01:
head = selfLoc.getHeading()
else:
head = hMath.RAD2DEG(math.atan2(-cov[1][0], cov[1][1] - sol))
else:
head = hMath.RAD2DEG(math.atan2(sol - cov[0][0], cov[0][1]))
# Find the beacon that is closest to the variance direction.
beaconPos = VisionLink.getGPSCoordArray()
curCompHead = 360
localiseBeacon = 0
for beacon in CHECK_BEACONS:
relPos = map(lambda b, s: b - s, beaconPos[beacon], selfLoc.getPos())
beaconHead = hMath.RAD2DEG(math.atan2(relPos[1], relPos[0]))
beaconDist = hMath.getLength(relPos)
localHead = hMath.normalizeAngle_180(beaconHead - selfLoc.getHeading())
if abs(localHead) < panLimit and beaconDist < distLimit:
compHead = abs(hMath.normalizeAngle_180(head - beaconHead))
if compHead > 90:
compHead = 180 - compHead
if compHead < curCompHead:
curCompHead = compHead
localiseBeacon = beacon
def saGoToTargetFacingHeading(targetX, targetY, targetH, \
maxSpeed = Action.MAX_FORWARD, maxTurn = Action.MAX_TURN):
# ariables relative to self localisation.
selfX = Global.selfLoc.getX()
selfY = Global.selfLoc.getY()
selfH = Global.selfLoc.getHeading()
relX = targetX - selfX
relY = targetY - selfY
relH = hMath.normalizeAngle_180(targetH - selfH)
relX += Constant.DOG_LENGTH/2.0/10.0*(math.cos(math.radians(selfH)) \
- math.cos(math.radians(targetH)))
relY += Constant.DOG_LENGTH/2.0/10.0*(math.sin(math.radians(selfH)) \
- math.sin(math.radians(targetH)))
relD = hMath.getLength((relX, relY))
distanceSquared = hMath.getDistSquaredBetween(targetX, targetY, selfX,
selfY)
inCircle = distanceSquared <= hMath.SQUARE(40)
faceH = hMath.getHeadingToFaceAt(Global.selfLoc, targetX, targetY)
##~ print "faceH: ", faceH
if not inCircle:
if abs(faceH) >= 30:
Action.walk(0, 0, faceH)
else:
#if sStealthDog.stealthDog(True):
# hFWHead.compulsoryAction = hFWHead.mustSeeBall
# hTrack.trackVisualBall()
# return
Action.walk(maxSpeed, 0, hMath.CLIP(faceH / 1.5, maxTurn))
else:
if relX == 0 and relY == 0:
# On the dog, math.atan2(0,0) give "Value Error: math domain error".
relTheta = 0
else:
relTheta = hMath.normalizeAngle_180(
hMath.RAD2DEG(math.atan2(relY, relX)) - selfH)
forward = hMath.CLIP(relD, maxSpeed) * math.cos(
hMath.DEG2RAD(relTheta))
left = hMath.CLIP(relD, maxSpeed) * math.sin(hMath.DEG2RAD(relTheta))
turnCCW = hMath.CLIP(relH, maxTurn)
Action.walk(forward, left, turnCCW)
def AlternateSetBeacon(panLimit=90, distLimit=550):
global localiseBeacon, lastBeacon
selfLoc = Global.selfLoc
beaconPos = getBeaconCoords()
curCompHead = panLimit
localiseBeacon = None
for beacon in beaconPos:
relPos = map(lambda b, s: b - s, beacon, selfLoc.getPos())
beaconHead = hMath.RAD2DEG(math.atan2(relPos[1], relPos[0]))
beaconDist = hMath.getLength(relPos)
localHead = hMath.normalizeAngle_180(beaconHead - selfLoc.getHeading())
if abs(localHead) < curCompHead and beaconDist < distLimit:
curCompHead = localHead
localiseBeacon = beacon
lastBeacon = localiseBeacon
def saGoToTarget(targetX, targetY, maxSpeed=None, maxTurn=None):
if maxSpeed == None:
maxSpeed = Action.MAX_FORWARD
if maxTurn == None:
maxTurn = Action.MAX_TURN
selfPos = Global.selfLoc.getPos()
selfH = Global.selfLoc.getHeading()
relX = targetX - selfPos[0]
relY = targetY - selfPos[1]
relD = hMath.getLength((relX, relY))
if relX == 0 and relY == 0:
relH = 0
else:
relH = hMath.normalizeAngle_180(
hMath.RAD2DEG(math.atan2(relY, relX)) - selfH)
forward = 0
left = 0
turnccw = 0
if relD < CLOSE_WALK_RANGE and abs(relH) > 50:
forward = relY
left = relX
turnccw = relH * 0.8
# print "close walk and turn",relY,left,turnccw
else:
if abs(relH) > 80: # stop and turn
turnccw = relH * 0.8
elif abs(relH) > 25: # walk and turn
turnccw = relH * 0.8
forward = hMath.CLIP(relD, maxSpeed) * math.cos(
hMath.DEG2RAD(relH))
# print "walk and turn",forward, 0, turnccw
else: # ellipticalwalk and turn
turnccw = hMath.CLIP(relH * 0.8, 25)
forward = hMath.CLIP(relD, maxSpeed) * math.cos(
hMath.DEG2RAD(relH))
# finally, we walk
Action.walk(forward, left, turnccw, minorWalkType=Action.SkeFastForwardMWT)
def saGoToTarget(targetX, targetY, maxSpeed = Action.MAX_FORWARD, \
maxTurn = Action.MAX_TURN):
selfX, selfY = Global.selfLoc.getPos()
selfH = Global.selfLoc.getHeading()
relX = targetX - selfX
relY = targetY - selfY
relD = hMath.getLength((relX, relY))
distanceSquared = hMath.getDistSquaredBetween(targetX, targetY, selfX,
selfY)
inCircle = distanceSquared <= hMath.SQUARE(40)
faceH = hMath.getHeadingToFaceAt(Global.selfLoc, targetX, targetY)
if not inCircle and abs(faceH) >= 30:
Action.walk(0, 0, hMath.CLIP(faceH, maxTurn))
elif not inCircle:
#if sStealthDog.stealthDog(True):
# hFWHead.compulsoryAction = hFWHead.mustSeeBall
# hTrack.trackVisualBall()
# return
Action.walk(maxSpeed, 0, hMath.CLIP(faceH / 1.5, maxTurn))
else:
if relX == 0 and relY == 0:
relTheta = 0
else:
relTheta = hMath.normalizeAngle_180(
hMath.RAD2DEG(math.atan2(relY, relX)) - selfH)
forward = hMath.CLIP(relD, maxSpeed) * math.cos(
hMath.DEG2RAD(relTheta))
left = hMath.CLIP(relD, maxSpeed) * math.sin(hMath.DEG2RAD(relTheta))
turnCCW = hMath.CLIP(relTheta, maxTurn)
Action.walk(forward, left, turnCCW)
def walk(target, selfPos, selfH):
relX = target[0] - selfPos[0]
relY = target[1] - selfPos[1]
relD = hMath.getLength((relX, relY))
if relX == 0 and relY == 0:
relThetaRad = 0
else:
relThetaRad = hMath.DEG2RAD(hMath.normalizeAngle_180(hMath.RAD2DEG(math.atan2(relY, relX)) - selfH))
sinTheta = math.sin(relThetaRad)
cosTheta = math.cos(relThetaRad)
leftSign = relD * sinTheta
leftAbs1 = abs(leftSign) #leftAbs1: the distance left component
if cosTheta != 0: #leftAbs2: the max capping due to max forward
leftAbs2 = abs(maxFwd * (sinTheta / cosTheta))
else:
leftAbs2 = 100000 # huge number
#print "left1:", leftAbs1, "left2:", leftAbs2
leftAbs = min(leftAbs1, leftAbs2, maxLeft)
left = hMath.getSign(leftAbs, leftSign)
if sinTheta != 0:
forward = left * (cosTheta / sinTheta)
else:
fwdSign = relD
fwdAbs = min(maxFwd, abs(relD))
forward = hMath.getSign(fwdAbs, fwdSign)
if turningLeft:
turnCCW = maxTurn
else:
turnCCW = -maxTurn
print __name__, forward, left, turnCCW
Action.walk(forward, left, turnCCW,walkType=Action.SkellipticalWalkWT)
def saGoToTargetFacingHeading(targetX, targetY, targetH, \
maxSpeed = None, maxTurn = None):
if maxSpeed == None:
maxSpeed = Action.MAX_FORWARD
if maxTurn == None:
maxTurn = Action.MAX_TURN
# Variables relative to self localisation.
selfPos = Global.selfLoc.getPos()
selfH = Global.selfLoc.getHeading()
relX = targetX - selfPos[0]
relY = targetY - selfPos[1]
relH = hMath.normalizeAngle_180(targetH - selfH)
# take into account that as we turn, the position of our head will change
relX += Constant.DOG_NECK_TO_BODY_CENTER_OFFSET * \
(math.cos(math.radians(selfH)) - math.cos(math.radians(targetH)))
relY += Constant.DOG_NECK_TO_BODY_CENTER_OFFSET * \
(math.sin(math.radians(selfH)) - math.sin(math.radians(targetH)))
relD = hMath.getLength((relX, relY))
if relX == 0 and relY == 0:
relTheta = 0
else:
relTheta = hMath.normalizeAngle_180(
hMath.RAD2DEG(math.atan2(relY, relX)) - selfH)
forward = hMath.CLIP(relD, maxSpeed) * math.cos(hMath.DEG2RAD(relTheta))
left = hMath.CLIP(relD, maxSpeed) * math.sin(hMath.DEG2RAD(relTheta))
turnccw = hMath.CLIP(relH, maxTurn)
Action.walk(forward,
left,
turnccw,
"ssd",
minorWalkType=Action.SkeFastForwardMWT)
def perform(dkd = 90, side = None, bx = None, by = None):
# This implementation is very similar to sGetBehindBall (based on 2003)
# but the ball is on the bottom edge of the circle, not the centre.
global onCircle
if side != None:
print "Warning: sGetBesideBall.perform: side is not yet implemented"
if bx == None or by == None:
(bx, by) = Global.gpsGlobalBall.getPos()
(myx, myy) = Global.selfLoc.getPos()
myh = Global.selfLoc.getHeading()
# Determine the centre of the circle, which is CIRCLE_RADIUS towards
# dkd from the ball. Global coords.
cx = bx + math.cos(hMath.DEG2RAD(dkd)) * CIRCLE_RADIUS
cy = by + math.sin(hMath.DEG2RAD(dkd)) * CIRCLE_RADIUS
# If we are backward of the ball or really close just run at it
ballRobotH = hMath.getHeadingToMe(bx, by, dkd, myx, myy)
ballDSquared = hMath.getDistSquaredBetween(myx, myy, bx, by)
if (abs(ballRobotH) > 90 or ballDSquared < hMath.SQUARE(20)):
Indicator.showHeadColor(Indicator.RGB_PURPLE)
ballH = hMath.getHeadingBetween(myx, myy, bx, by)
hTrack.saGoToTargetFacingHeading(bx, by, ballH)
return
# Work out if we are left or right of the centre (relative to DKD as 0)
robotH = hMath.getHeadingToMe(cx, cy, dkd, myx, myy)
# FIXME: allow choice of direction
if (robotH > 0): # robot to the left
#print "robot to left of ball",
direction = Constant.dANTICLOCKWISE
else: # robot to the right
#print "robot to right of ball",
direction = Constant.dCLOCKWISE
# The circling point can be calculated as looking from the centre
# towards the robot at CIRCLE_DEGREES to the left/right, and distance
# CIRCLE_RADIUS. CircleAng is from the centre facing the robot with
# positive x at zero degrees.
# There are two modes here. In the first we are well outside the and
# running to make a tangent with the circle. In the second we are close
# to or inside the circle and tracing the circumference
centreDSquared = hMath.getDistSquaredBetween(myx, myy, cx, cy)
if (centreDSquared > hMath.SQUARE(CIRCLE_RADIUS + 20)):
#print "Outside circle, running to tangent"
onCircle = False
Indicator.showHeadColor(Indicator.RGB_GREEN)
if direction == Constant.dANTICLOCKWISE:
circleAng = 90 + CIRCLE_DEGREES
else:
circleAng = 90 - CIRCLE_DEGREES
circleAng = hMath.normalizeAngle_180(circleAng)
else:
#print "On circle, tracing circumference"
onCircle = True
Indicator.showHeadColor(Indicator.RGB_YELLOW)
if direction == Constant.dANTICLOCKWISE:
circleAng = 110
else:
circleAng = 70
# print "me", int(myx), int(myy), "ball", int(bx), int(by), \
# "centre", int(cx), int(cy), "robotH", int(robotH),
# relative to centre facing robot
circleRelX = math.cos(hMath.DEG2RAD(circleAng)) * CIRCLE_RADIUS
circleRelY = math.sin(hMath.DEG2RAD(circleAng)) * CIRCLE_RADIUS
#print "circleAng", circleAng, "rel circle pos", circleRelX, circleRelY
robotH = hMath.normalizeAngle_180(robotH + dkd) # now global
(circleX, circleY) = hMath.getGlobalCoordinate(cx, cy, robotH, \
circleRelX, circleRelY)
# print "gRobotH", robotH, "circle pos", int(circleX), int(circleY)
# circleX/Y now is the global coords of the circle point, so walk there.
# ballH = hMath.getHeadingBetween(myx, myy, bx, by) # global
if onCircle:
# Calls the walk directly to ensure smoothness: no stopping to turn
relX = circleX - myx
relY = circleY - myy
relD = hMath.getLength((relX, relY))
relTheta = hMath.RAD2DEG(hMath.getHeadingToRelative(relX, relY))
# Don't turn outwards much, even if we are inside the circle. Walking
# forward will put us back on it. Nobu can you fix this?
# print "relTheta", relTheta, "=>",
# if direction == Constant.dANTICLOCKWISE and relTheta < 0:
# #relTheta = hMath.CLIP(relTheta, 15)
# relTheta = 0
# elif direction == Constant.dCLOCKWISE and relTheta > 0:
# #relTheta = hMath.CLIP(relTheta, 15)
# relTheta = 0
# print relTheta
left = 0
if abs(relTheta) < 30:
Action.walk(Action.MAX_FORWARD, left, relTheta)
else:
Action.walk(Action.MAX_FORWARD, left, relTheta)
else:
hTrack.saGoToTarget(circleX, circleY)
def SmartSetBeacon(panLimit=90, distLimit=550):
global localiseBeacon, lastBeacon
# Get shared variables from elsewhere.
selfLoc = Global.selfLoc
cov = VisionLink.getGPSSelfCovariance()
# Calculate the direction of the larger axis of the self covariance
b = -(cov[0][0] + cov[1][1])
c = cov[0][0] * cov[1][1] - cov[1][0] * cov[1][0]
det = b * b - 4 * c
if (det < 0.00001):
sol = -b / 2
else:
sol = (-b + math.sqrt(det)) / 2
# chooseCov = False # Are we choosing a beacon to reduce covariance?
if abs(cov[0][0] - sol) < 0.01 and cov[0][1] < 0.01:
# If self position variance is circular, all beacons are equally good
# to localise off, and so choose the one closest to current heading.
# dHead is global heading we desire to stay close to
if abs(cov[1][1] - sol) < 0.01:
dHead = selfLoc.getHeading()
else:
dHead = hMath.RAD2DEG(math.atan2(-cov[1][0], cov[1][1] - sol))
else:
#chooseCov = True
dHead = hMath.RAD2DEG(math.atan2(sol - cov[0][0], cov[0][1]))
# Find the beacon that is closest to the variance direction.
beaconPos = getBeaconCoords()
bestHead = nextBestHead = 360
localiseBeacon = None # a tuple (x, y)
nextBestBeacon = None
#print beaconPos
for beacon in beaconPos:
relPos = map(lambda b, s: b - s, beacon, selfLoc.getPos())
beaconHead = hMath.RAD2DEG(math.atan2(relPos[1], relPos[0]))
beaconDist = hMath.getLength(relPos)
localHead = hMath.normalizeAngle_180(beaconHead - selfLoc.getHeading())
if abs(localHead) < panLimit and beaconDist < distLimit:
#print beacon, localHead, beaconHead, dHead
compHead = abs(hMath.normalizeAngle_180(dHead - beaconHead))
if compHead > 90:
compHead = 180 - compHead
if compHead < bestHead:
nextBestBeacon = localiseBeacon
nextBestHead = bestHead
localiseBeacon = beacon
bestHead = compHead
elif compHead < nextBestHead:
nextBestBeacon = beacon
nextBestHead = compHead
#print "sActiveLocalise best =", localiseBeacon, "next =", nextBestBeacon
# Don't choose the same beacon twice to reduce covariance. If we chose it
# last time based on covariance and the covariance is still large that way
# we likely didn't see it, so choose the next best
if localiseBeacon == lastBeacon and nextBestBeacon != None:
# print "Choosing a different beacon to last time"
localiseBeacon = nextBestBeacon
nextBestBeacon = None
# Prefer not to look at a really close beacon - we don't get good
# information
if nextBestBeacon != None and \
hMath.getDistanceBetween(selfLoc.getX(), selfLoc.getY(),\
localiseBeacon[0], localiseBeacon[1]) < MIN_BEACON_DIST:
# print "Choosing not to use close beacon"
localiseBeacon = nextBestBeacon
#print "Choosing beacon at", localiseBeacon, "frame", Global.frame
lastBeacon = localiseBeacon
if localiseBeacon == None:
return False
else:
return True