Python MyMath.getRelativeX方法代码示例

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

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

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import getRelativeX [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, relH = 0, 0, 0
    if hasattr(dest, "relX") and \
           hasattr(dest, "relY"):
        relX = dest.relX
        relY = dest.relY
    else:
        bearingDeg = my.getRelativeBearing(dest)
        distToDest = my.distTo(dest)
        relX = MyMath.getRelativeX(distToDest, bearingDeg)
        relY = MyMath.getRelativeY(distToDest, bearingDeg)

    if hasattr(dest, "relH"):
        relH = dest.relH
    elif hasattr(dest, "bearing"):
        relH = dest.bearing
    else:
        relH = MyMath.sub180Angle(dest.h - my.h)

    # calculate forward speed
    forwardGain = constants.APPROACH_X_WITH_GAIN_DIST
    sX = relX * forwardGain
    if fabs(sX) < constants.OMNI_MIN_X_MAGNITUDE:
        sX = 0
    else:
        sX = MyMath.clip(sX,
                         constants.OMNI_REV_MAX_SPEED,
                         constants.OMNI_FWD_MAX_SPEED)

    # calculate sideways speed
    strafeGain = constants.APPROACH_Y_WITH_GAIN_DIST
    sY = relY * strafeGain
    if fabs(sY) < constants.OMNI_MIN_Y_MAGNITUDE:
        sY = 0
    else:
        sY = MyMath.clip(sY,
                         constants.OMNI_RIGHT_MAX_SPEED,
                         constants.OMNI_LEFT_MAX_SPEED,)

    # calculate spin speed
    if (fabs(relH) < 10.0):
        sTheta = 0.0
    else:
        spinGain = constants.APPROACH_THETA_WITH_GAIN_DIST
        sTheta = relH * spinGain
        sTheta = MyMath.clip(sTheta,
                             constants.OMNI_MAX_RIGHT_SPIN_SPEED,
                             constants.OMNI_MAX_LEFT_SPIN_SPEED)

    denom = sqrt(sX*sX + sY*sY + sTheta*sTheta) / constants.OMNI_GAIN
    if denom != 0:
        sX     /= denom
        sY     /= denom
        sTheta /= denom

    return (sX, sY, sTheta)

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import getRelativeX [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, relH = 0, 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 = 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.OMNI_REV_MAX_SPEED,
                         constants.OMNI_FWD_MAX_SPEED)

    # calculate sideways speed
    strafeGain = 15. / constants.APPROACH_Y_WITH_GAIN_DIST # 15cm/sec in y direction
    sY = relY * strafeGain
    if fabs(sY) < constants.OMNI_MIN_Y_MAGNITUDE:
        sY = 0
    else:
        sY = MyMath.clip(sY,
                         constants.OMNI_RIGHT_MAX_SPEED,
                         constants.OMNI_LEFT_MAX_SPEED,)

    # calculate spin speed
    if (fabs(relH) < 25.0):
        sTheta = 0.0
    else:
        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)

    # refine x and y speeds
    if (fabs(relH) > 50):
        sX = 0
        sY = 0

    elif (fabs(relH) > 35):
        sY = 0

    return (sX, sY, sTheta)

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import getRelativeX [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 getRelativeX [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 getRelativeX [as 别名]
def getWalkSpinParam(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 forward speed
    forwardGain = 1./constants.APPROACH_X_WITH_GAIN_DIST
    sX = relX * forwardGain
    if fabs(sX) < constants.OMNI_MIN_X_MAGNITUDE:
        sX = 0
    else:
        sX = MyMath.clip(sX,
                         constants.OMNI_REV_MAX_SPEED,
                         constants.OMNI_FWD_MAX_SPEED)

    # calculate spin speed
    if (fabs(relH) < 5.0):
        sTheta = 0.0
    else:
        spinGain = 1./constants.APPROACH_THETA_WITH_GAIN_DIST
        sTheta = relH * spinGain
        sTheta = MyMath.clip(sTheta,
                             constants.OMNI_MAX_RIGHT_SPIN_SPEED,
                             constants.OMNI_MAX_LEFT_SPIN_SPEED)
    # Correct sX
    if fabs(relH)  > 50.:
        sX = 0

    return (sX, 0, sTheta)