Python MyMath.sign方法代码示例

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import sign [as 别名]
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()

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import sign [as 别名]
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()

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import sign [as 别名]
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()

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import sign [as 别名]
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()

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import sign [as 别名]
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()

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import sign [as 别名]
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)

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import sign [as 别名]
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()

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import sign [as 别名]
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()

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import sign [as 别名]
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)

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import sign [as 别名]
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)

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import sign [as 别名]
def getOmniWalkParam(my, dest):
    # we use distance and bearing to get relX, relY which we already have
    # for the ball. be nice not to recalculate it.
    relX, relY = 0, 0

    if hasattr(dest, "relX") and \
            hasattr(dest, "relY") and \
            hasattr(dest, "relH"):
        relX = dest.relX
        relY = dest.relY
        relH = dest.relH

    else:
        bearingDeg = my.getRelativeBearing(dest)
        distToDest = my.distTo(dest)
        relX = MyMath.getRelativeX(distToDest, bearingDeg)
        relY = MyMath.getRelativeY(distToDest, bearingDeg)
        relH = MyMath.sub180Angle(dest.h - my.h)

    # calculate forward speed
    forwardGain = constants.OMNI_GOTO_X_GAIN * relX
    sX = constants.OMNI_GOTO_FORWARD_SPEED * forwardGain
    sX = MyMath.clip(sX,
                     constants.OMNI_MIN_X_SPEED,
                     constants.OMNI_MAX_X_SPEED)
    if fabs(sX) < constants.OMNI_MIN_X_MAGNITUDE:
        sX = 0

    # calculate sideways speed
    strafeGain = constants.OMNI_GOTO_Y_GAIN * relY
    sY = constants.OMNI_GOTO_STRAFE_SPEED  * strafeGain
    sY = MyMath.clip(sY,
                     constants.OMNI_MIN_Y_SPEED,
                     constants.OMNI_MAX_Y_SPEED,)
    if fabs(sY) < constants.OMNI_MIN_Y_MAGNITUDE:
        sY = 0

    # calculate spin speed
    spinGain = constants.GOTO_SPIN_GAIN
    hDiff = MyMath.sub180Angle(dest.h - my.h)

    if (fabs(hDiff) < 2.0):
        sTheta = 0.0
    else:
        sTheta = MyMath.sign(hDiff) * getRotScale(hDiff) * \
                 constants.OMNI_MAX_SPIN_SPEED * spinGain

        sTheta = MyMath.clip(sTheta,
                             constants.OMNI_MIN_SPIN_SPEED,
                             constants.OMNI_MAX_SPIN_SPEED)

    return (sX, sY, sTheta)

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import sign [as 别名]
def getSpinOnlyParam(my, dest):
    # Determine the speed to turn
    # see if getRotScale can go faster

    headingDiff = my.getRelativeBearing(dest)
    if (fabs(headingDiff) < 25.0):
        sTheta = 0.0
    else:
        sTheta = MyMath.sign(headingDiff) * constants.MAX_SPIN_MAGNITUDE * \
                 getRotScale(headingDiff)

    sX, sY = 0, 0
    return (sX, sY, sTheta)

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import sign [as 别名]
def ballInMyBox(player):
    if player.firstFrame():
        player.brain.tracker.activeLoc()
        player.brain.CoA.setRobotGait(player.brain.motion)

    ball = player.brain.ball
    if fabs(ball.bearing) > constants.BALL_APPROACH_BEARING_THRESH:
        player.setSpeed(0, 0, constants.BALL_SPIN_SPEED *
                        MyMath.sign(ball.bearing) )
    elif fabs(ball.bearing) < constants.BALL_APPROACH_BEARING_OFF_THRESH :
        player.stopWalking()
    if not player.ballInMyGoalBox():
        return player.goLater('chase')
    return player.stay()

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import sign [as 别名]
def getWalkSpinParam(my, dest):

    relX = 0
    bearingDeg = my.getRelativeBearing(dest)
    distToDest = my.distTo(dest)
    if hasattr(dest, "relX"):
        relX = dest.relX
    else:
        relX = MyMath.getRelativeX(distToDest, bearingDeg)

    # calculate ideal max forward speed
    sX = constants.GOTO_FORWARD_SPEED * MyMath.sign(relX)

    if (fabs(bearingDeg) < 2.0):
        sTheta = 0.0
    else:
        # calculate ideal max spin speed
        sTheta = (MyMath.sign(bearingDeg) * getRotScale(bearingDeg) *
                  constants.OMNI_MAX_SPIN_SPEED)

    absSTheta = fabs(sTheta)

    if  fabs(bearingDeg) > 20:
        sX = MyMath.clip(sX,
                         constants.OMNI_MIN_X_SPEED,
                         constants.OMNI_MAX_X_SPEED)
        sTheta = MyMath.sign(sTheta)* constants.OMNI_MAX_SPIN_SPEED

    elif fabs(bearingDeg)  > 35:
        sX = 0
        sTheta = constants.MAX_SPIN_SPEED * MyMath.sign(sTheta)

    gain = 1.0
    if distToDest < ChaseBallConstants.APPROACH_WITH_GAIN_DIST:
        gain = constants.GOTO_CLOSE_GAIN

    return (sX * gain, 0, sTheta * gain)

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import sign [as 别名]
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()