Python util.MyMath类代码示例

def orbitBeforeKick(player):
    """
    State for circling the ball when we're facing our goal.
    """
    brain = player.brain
    my = brain.my
    if player.firstFrame():
        player.orbitStartH = my.h
        brain.CoA.setRobotGait(brain.motion)
        brain.tracker.trackBall()

        shotPoint = KickingHelpers.getShotCloseAimPoint(player)
        bearingToGoal = MyMath.getRelativeBearing(my.x, my.y, my.h,
                                                  shotPoint[0],
                                                  shotPoint[1] )
        spinDir = -MyMath.sign(bearingToGoal)
        player.brain.nav.orbitAngle(spinDir * 90)
    if not player.brain.tracker.activeLocOn and \
            transitions.shouldScanFindBall(player):
        player.brain.CoA.setRobotGait(player.brain.motion)
        return player.goLater('scanFindBall')
    elif brain.ball.dist > constants.STOP_ORBIT_BALL_DIST:
        return player.goLater('chase')

    if player.brain.nav.isStopped() and not player.firstFrame():
        return player.goLater('positionForKick')
    return player.stay()

def spinToBall(player):
    """
    State to spin to turn to the ball
    """
    if player.firstFrame():
        player.stopWalking()
        player.brain.tracker.trackBall()

    ball = player.brain.ball

    turnRate = MyMath.clip(ball.locBearing*ChaseConstants.BALL_SPIN_GAIN,
                           -ChaseConstants.BALL_SPIN_SPEED,
                           ChaseConstants.BALL_SPIN_SPEED)

    if transitions.atSpinBallDir(player):
        return player.goLater('atSpinBallPosition')

    elif transitions.shouldSpinFindBallPosition(player):
        return player.goLater('spinFindBallPosition')

    elif player.currentSpinDir != MyMath.sign(turnRate):
        player.stopWalking()
        player.currentSpinDir = MyMath.sign(turnRate)
    elif player.stoppedWalk and ball.on and player.brain.nav.isStopped():
        player.setWalk(x=0,y=0,theta=turnRate)

    return player.stay()

def turnToBall(player):
    """
    Rotate to align with the ball. When we get close, we will approach it
    """
    ball = player.brain.ball

    if player.firstFrame():
        player.hasAlignedOnce = False
        player.brain.tracker.trackBall()
        player.brain.CoA.setRobotGait(player.brain.motion)

    # Determine the speed to turn to the ball
    turnRate = MyMath.clip(ball.bearing*constants.BALL_SPIN_GAIN,
                           -constants.BALL_SPIN_SPEED,
                           constants.BALL_SPIN_SPEED)

    # Avoid spinning so slowly that we step in place
    if fabs(turnRate) < constants.MIN_BALL_SPIN_MAGNITUDE:
        turnRate = MyMath.sign(turnRate)*constants.MIN_BALL_SPIN_MAGNITUDE

    if ball.on:
        player.setSpeed(x=0,y=0,theta=turnRate)

    if transitions.shouldKick(player):
        return player.goNow('waitBeforeKick')
    elif transitions.shouldPositionForKick(player):
        return player.goNow('positionForKick')
    elif transitions.shouldApproachBall(player):
        return player.goLater('approachBall')
    elif transitions.shouldScanFindBall(player):
        return player.goLater('scanFindBall')

    return player.stay()

def getWalkStraightParam(my, dest):

    distToDest = my.distTo(dest)

    if distToDest < ChaseBallConstants.APPROACH_WITH_GAIN_DIST:
        gain = constants.GOTO_FORWARD_GAIN * distToDest
    else:
        gain = 1.0

    sX = MyMath.clip(constants.GOTO_FORWARD_SPEED*gain,
                     constants.WALK_TO_MIN_X_SPEED,
                     constants.WALK_TO_MAX_X_SPEED)

    bearingDeg= my.getRelativeBearing(dest)

    if (fabs(bearingDeg) < 2.0):
        sTheta = 0.0
    else:
        sTheta = MyMath.clip(MyMath.sign(bearingDeg) *
                             constants.GOTO_STRAIGHT_SPIN_SPEED *
                             getRotScale(bearingDeg),
                             -constants.GOTO_STRAIGHT_SPIN_SPEED,
                             constants.GOTO_STRAIGHT_SPIN_SPEED )
    sY = 0

    return (sX, sY, sTheta)

def getWalkBackParam(my, dest):
    relX, relH = 0, 0
    if hasattr(dest, "relX") and \
            hasattr(dest, "relH"):
        relX = dest.relX
        relH = dest.relH
    else:
        bearingDeg = my.getRelativeBearing(dest)
        distToDest = my.distTo(dest)
        relX = MyMath.getRelativeX(distToDest, bearingDeg)
        relH = MyMath.sub180Angle(dest.h - my.h)

    if not fabs(relH) > 150 :
        spinGain = constants.APPROACH_THETA_WITH_GAIN_DIST
        sTheta = (180-relH) * spinGain
        sTheta = MyMath.clip(sTheta,
                             constants.OMNI_MAX_RIGHT_SPIN_SPEED,
                             constants.OMNI_MAX_LEFT_SPIN_SPEED)
        return ( 0, 0, sTheta)

    forwardGain = constants.APPROACH_X_WITH_GAIN_DIST
    sX = relX * forwardGain
    sX = MyMath.clip(sX,
                     constants.GOTO_BACKWARD_SPEED,
                     constants.GOTO_FORWARD_SPEED)

    return (sX, 0, 0)

def getWalkStraightParam(my, dest):
    relX, relH = 0, 0
    if hasattr(dest, "relX") and \
            hasattr(dest, "relH"):
        relX = dest.relX
        relH = dest.relH
    else:
        bearingDeg = my.getRelativeBearing(dest)
        distToDest = my.distTo(dest)
        relX = MyMath.getRelativeX(distToDest, bearingDeg)
        relH = MyMath.sub180Angle(dest.h - my.h)

    # calculate spin speed
    if (fabs(relH) < 25.0):
        sTheta = 0.0
    else: #spin first
        spinGain = 20. / constants.APPROACH_THETA_WITH_GAIN_DIST # 20degs/sec in theta
        sTheta = relH * spinGain
        sTheta = MyMath.clip(sTheta,
                             constants.OMNI_MAX_RIGHT_SPIN_SPEED,
                             constants.OMNI_MAX_LEFT_SPIN_SPEED)
        return (0, 0, sTheta)

   # calculate forward speed if h is good.
    forwardGain = 20. / constants.APPROACH_X_WITH_GAIN_DIST # 20cm/sec in x direction
    sX = relX * forwardGain
    if fabs(sX) < constants.OMNI_MIN_X_MAGNITUDE:
        sX = 0
    else:
        sX = MyMath.clip(sX,
                         constants.GOTO_BACKWARD_SPEED,
                         constants.GOTO_FORWARD_SPEED)

    return (sX, 0, 0)

def pfk_final(nav):
    """
    we're done spinning and aligned for y.
    approach to final relX with x only.
    (may need to accept minor y changes in future)
    """

    ball = nav.brain.ball

    (x_offset, y_offset, heading) = nav.kick.getPosition()

    x_diff = ball.relX - x_offset

    # arbitrary
    if fabs(x_diff) < PFK_CLOSE_ENOUGH_XY:
        sX = 0.0
    else:
        sX = MyMath.clip(x_diff * PFK_X_GAIN,
                         PFK_MIN_X_SPEED,
                         PFK_MAX_X_SPEED)
        sX = max(PFK_MIN_X_MAGNITUDE,sX) * MyMath.sign(sX)

    helper.setSlowSpeed(nav,sX, 0.0, 0.0)

    # kicking time!
    if sX == 0.0:
        return nav.goNow('stop')

    return nav.stay()

def approachBallWalk(player):
    """
    Method that is used by both approach ball and dribble
    We use things as to when we should leave and how we should walk
    """

    if player.brain.playbook.role != pbc.GOALIE:
        if transitions.shouldNotGoInBox(player):
            return player.goLater('ballInMyBox')
        elif transitions.shouldChaseAroundBox(player):
            return player.goLater('chaseAroundBox')
        elif transitions.shouldApproachBallWithLoc(player):
            return player.goNow('approachBallWithLoc')
        elif transitions.shouldTurnToBall_ApproachBall(player):
            return player.goLater('turnToBall')
        elif not player.brain.tracker.activeLocOn and \
                transitions.shouldScanFindBall(player):
            return player.goLater('scanFindBall')
        elif player.brain.tracker.activeLocOn and \
                transitions.shouldScanFindBallActiveLoc(player):
            return player.goLater('scanFindBall')
        elif transitions.shouldAvoidObstacleDuringApproachBall(player):
            return player.goLater('avoidObstacle')

    # Determine our speed for approaching the ball
    ball = player.brain.ball
    if player.brain.playbook.role == pbc.GOALIE and goalTran.dangerousBall(player):
        return player.goNow('approachDangerousBall')

    if ball.dist < constants.APPROACH_WITH_GAIN_DIST:
        sX = MyMath.clip(ball.dist*constants.APPROACH_X_GAIN,
                         constants.MIN_APPROACH_X_SPEED,
                         constants.MAX_APPROACH_X_SPEED)
    else :
        sX = constants.MAX_APPROACH_X_SPEED

    # Determine the speed to turn to the ball
    sTheta = MyMath.clip(ball.bearing*constants.APPROACH_SPIN_GAIN,
                         -constants.APPROACH_SPIN_SPEED,
                         constants.APPROACH_SPIN_SPEED)
    # Avoid spinning so slowly that we step in place
    if fabs(sTheta) < constants.MIN_APPROACH_SPIN_MAGNITUDE:
        sTheta = 0.0

    # Set our walk towards the ball
    if ball.on:
        player.setSpeed(sX,0,sTheta)

    return player.stay()

def positionForKick(player):
    """
    State to align on the ball once we are near it
    """
    if player.firstFrame():
        player.brain.CoA.setRobotSlowGait(player.brain.motion)

    ball = player.brain.ball

    player.inKickingState = True
    # Leave this state if necessary
    if transitions.shouldKick(player):
        player.brain.CoA.setRobotGait(player.brain.motion)
        return player.goNow('waitBeforeKick')
    elif transitions.shouldScanFindBall(player):
        player.inKickingState = False
        player.brain.CoA.setRobotGait(player.brain.motion)
        return player.goLater('scanFindBall')
    elif transitions.shouldTurnToBallFromPositionForKick(player):
        player.inKickingState = False
        player.brain.CoA.setRobotGait(player.brain.motion)
        return player.goLater('turnToBall')
    elif transitions.shouldApproachFromPositionForKick(player):
        player.inKickingState = False
        player.brain.CoA.setRobotGait(player.brain.motion)
        return player.goLater('approachBall')

    # Determine approach speed
    targetY = ball.relY

    sY = MyMath.clip(targetY * constants.PFK_Y_GAIN,
                     constants.PFK_MIN_Y_SPEED,
                     constants.PFK_MAX_Y_SPEED)

    sY = max(constants.PFK_MIN_Y_MAGNITUDE,sY) * MyMath.sign(sY)

    if transitions.shouldApproachForKick(player):
        targetX = (ball.relX -
                   (constants.BALL_KICK_LEFT_X_CLOSE +
                    constants.BALL_KICK_LEFT_X_FAR) / 2.0)
        sX = MyMath.clip(ball.relX * constants.PFK_X_GAIN,
                         constants.PFK_MIN_X_SPEED,
                         constants.PFK_MAX_X_SPEED)
    else:
        sX = 0.0

    if ball.on:
        player.setSpeed(sX,sY,0)
    return player.stay()

def penaltyRelocalize(player):
    if player.firstFrame():
        player.setWalk(1, 0, 0)

    if player.brain.ball.on:
        player.brain.tracker.trackBall()
        return player.goLater('gamePlaying')

    if player.brain.my.locScore == NogginConstants.GOOD_LOC or \
            player.brain.my.locScore == NogginConstants.OK_LOC:
        player.shouldRelocalizeCounter += 1

        if player.shouldRelocalizeCounter > 30:
            player.shouldRelocalizeCounter = 0
            return player.goLater('gamePlaying')

    else:
        player.shouldRelocalizeCounter = 0

    if not player.brain.motion.isHeadActive():
        player.brain.tracker.locPans()

    if player.counter > constants.RELOC_SPIN_FRAME_THRESH:
        direction = MyMath.sign(player.getWalk()[2])
        if direction == 0:
            direction = 1

        player.setWalk(0 , 0, constants.RELOC_SPIN_SPEED * direction)

    return player.stay()

def atHeading(my, targetHeading):
    """
    Returns true if we are at a heading close enough to what we want
    """
    hDiff = fabs(MyMath.sub180Angle(my.h - targetHeading))
    return hDiff < constants.CLOSE_ENOUGH_H and \
           my.uncertH < constants.LOC_IS_ACTIVE_H

def relocalize(player):
    if player.firstFrame():
        player.setWalk(constants.RELOC_X_SPEED, 0, 0)

    if player.brain.my.locScore != NogginConstants.locScore.BAD_LOC:
        player.shouldRelocalizeCounter += 1

        if player.shouldRelocalizeCounter > 30:
            player.shouldRelocalizeCounter = 0
            return player.goLater(player.lastDiffState)

    else:
        player.shouldRelocalizeCounter = 0

    if not player.brain.motion.isHeadActive():
        player.brain.tracker.locPans()

    if player.counter > constants.RELOC_SPIN_FRAME_THRESH:
        direction = MyMath.sign(player.getWalk()[2])
        if direction == 0:
            direction = 1

        player.setWalk(0, 0, constants.RELOC_SPIN_SPEED * direction)

    return player.stay()

def pfk_xy(nav):
    """
    ball bearing is outside safe limit, we're in danger of losing the ball.
    position x,y only
    """

    ball = nav.brain.ball
    if ball.relX < SAFE_BALL_REL_X and \
           ball.dist < SAFE_TO_STRAFE_DIST:
        return nav.goNow('pfk_x')

    """
    if fabs(ball.bearing) < START_SPIN_BEARING:
        print "bearing to ball: %g" % ball.bearing
        return nav.goNow('pfk_all')
    """

    (x_offset, y_offset, heading) = nav.kick.getPosition()
    target_y = ball.relY - y_offset

    # arbitrary
    if fabs(target_y) < PFK_CLOSE_ENOUGH_XY:
        return nav.goNow('pfk_final')
    else:
        sY = MyMath.clip(target_y * PFK_Y_GAIN,
                         PFK_MIN_Y_SPEED,
                         PFK_MAX_Y_SPEED)
        sY = max(PFK_MIN_Y_MAGNITUDE,sY) * MyMath.sign(sY)

    x_diff = ball.relX - SAFE_BALL_REL_X
    # arbitrary
    if fabs(x_diff) < PFK_CLOSE_ENOUGH_XY:
        sX = 0.0
    else:
        sX = MyMath.clip(x_diff * PFK_X_GAIN,
                         PFK_MIN_X_SPEED,
                         PFK_MAX_X_SPEED)
        sX = max(PFK_MIN_X_MAGNITUDE,sX) * MyMath.sign(sX)

    # in position, let's kick the ball!
    if (sX == 0.0 and sY == 0.0):
        return nav.goNow('stop')

    helper.setSlowSpeed(nav,sX,sY,0.0)

    return nav.stay()

def setSpeed(nav, x, y, theta):
    """
    Wrapper method to easily change the walk vector of the robot
    """
    walk = motion.WalkCommand(x=x,y=y,theta=theta)
    nav.brain.motion.setNextWalkCommand(walk)

    nav.walkX, nav.walkY, nav.walkTheta = x, y, theta
    nav.curSpinDir = MyMath.sign(theta)

def step(nav, x, y, theta, numSteps):
    """
    Wrapper method to easily change the walk vector of the robot
    """
    steps = motion.StepCommand(x=x,y=y,theta=theta,numSteps=numSteps)
    nav.brain.motion.sendStepCommand(steps)

    nav.walkX, nav.walkY, nav.walkTheta = x, y, theta
    nav.curSpinDir = MyMath.sign(theta)