checkPos.py

Go to the documentation of this file.

from __future__ import division, absolute_import

import numpy
import coordConv

import tcc.base

__all__ = ["checkPos"]

def checkPos(pos, minPos, maxPos, nearPos, wrapPref, doWrap):
    """Apply wrap preference and verify that position is in bounds
    
    @param[in] pos  position, wrap unknown
    @param[in] minPos  minimum allowed position
    @param[in] maxPos  maximum allowed position
    @param[in] nearPos  nearby angle (the reference for near wrap or doWrap)
    @param[in] doWrap:
        if true (slewing): use wrapPref and unwrap as desired, avoiding limits
        if false (tracking): ignore wrapPref and only wrap near, even if out of bounds
    @param[in] wrapPref  wrap preference; ignored if doWrap false
    @return two items:
    - outPos: wrapped position, or nan if errCode not AxisErr_OK
    - errCode: one of:
        - AxisErr_OK if no error
        - AxisErr_MinPos if pos < minPos
        - AxisErr_MaxPos if pos > maxPos
        - AxisErr_CannotCompute if wrapPref = None and nearPos is not finite

    @throw RuntimeError if wrapPref unknown (if doWrap true)

    @warning: If the limits of motion span a range greater than 720 degrees,
    this routine will not use the excess, unless doWrap false or wrapPref is "near".
    See "details" for more information about the ranges used.

    Details:
    The limits minPos and maxPos are inclusive, meaning
    outPos may equal either limit and still be in bounds.

    The following ranges are used:
    Wrap Preference     Range
    doWrap false        [nearPos - 180, nearPos + 180)
    WrapType_None       no change; outPos = pos

                        Ideal Range (may be adjusted to avoid limits)
    WrapType_Nearest    [nearPos - 180, nearPos + 180)
    WrapType_Negative   [meanPos - 360, meanPos)
    WrapType_Middle     [meanPos - 180, meanPos + 180)
    WrapType_Positive   [meanPos, meanPos + 360)

    where:
    - meanPos = (minPos + maxPos) / 2
    - [,) means the lower limit is inclusive and the upper exclusive.
    - Ideal Range means the range if limits are not an issue.
      If the upper end is out of bounds, both ends are decreased.
      Then if the lower end is out of bounds, both ends are increased.
      Then the position is wrapped and range checked.

    The main equation:
    Given an angle X and a range [A, A + 360),
    the angle WrapX = X wrapped into the given range is given by:
    WrapX = Y, if Y >= A, Y + 360 otherwise,
    where
    Y = ((X - A) mod 360) + A
    and "mod" is the mathematical modulo function,
    which is similar to the FORTRAN MOD function except that
    S mod T is always in the range [0, T) (T > 0)
    """
    # default outputs
    outPos = pos
    errCode = tcc.base.AxisErr_OK
    
    if not numpy.isfinite(pos):
        return (pos, tcc.base.AxisErr_CannotCompute)

    # handle NoUnwrap and None cases
    if (not doWrap) or (wrapPref == tcc.base.WrapType_None):
        # do not wrap; either wrap to nearest or accept position "as is"
        if doWrap:
            # wrapPref is None; accept position "as is"
            outPos = pos
        else:
            # tracking: wrap to nearest, even if going out of bounds
            outPos = coordConv.wrapNear(pos, nearPos)

        if outPos < minPos:
            errCode = tcc.base.AxisErr_MinPos
        elif outPos > maxPos:
            errCode = tcc.base.AxisErr_MaxPos
    
    else:
        # apply wrap preference; avoid hitting the limits if possible
        # compute wrapMin: the ideal minimum angle of the wrap range
        meanPos = (minPos + maxPos) / 2.0
        if wrapPref == tcc.base.WrapType_Nearest:
            wrapMin = nearPos - 180.0
        elif wrapPref == tcc.base.WrapType_Negative:
            wrapMin = meanPos - 360.0
        elif wrapPref == tcc.base.WrapType_Middle:
            wrapMin = meanPos - 180.0
        elif wrapPref == tcc.base.WrapType_Positive:
            wrapMin = meanPos
        else:
            raise RuntimeError("Unknown wrap preference %s" % (wrapPref,))
            
        if not numpy.isfinite(wrapMin):
            return (coordConv.DoubleNaN, tcc.base.AxisErr_CannotCompute)

        # Adjust the minimum of the wrap range for the limits, if necessary:
        # If the upper end is out of bounds, decrease the lower end
        # to maxPos - 360, to preserve a full 360 deg of motion.
        # Then if the lower end is out of bounds, increase it to minPos.
        # After adjustment, the minimum (wrapMin) is guranteed >= minPos,
        # but the maximum (wrapMin + 360) is NOT guranteed <= maxPos.
        wrapMin = min(wrapMin, maxPos - 360.0)
        wrapMin = max(wrapMin, minPos)

        # put the angle into the wrap range, which is [wrapMin, wrapMin + 360)
        diffMod360 = (pos - wrapMin) % 360.0
        if diffMod360 < 0.0:
            diffMod360 = diffMod360 + 360.0

        outPos = diffMod360 + wrapMin

        # if the resulting angle is too large,
        # we cannot find a wrap that is in range
        # set output position = input position and set errCode accordingly
        if outPos > maxPos:
            outPos = pos
            if outPos < minPos:
                errCode = tcc.base.AxisErr_MinPos
            else:
                errCode = tcc.base.AxisErr_MaxPos

    if errCode != tcc.base.AxisErr_OK:
        outPos = float("nan")
    
    return (outPos, errCode)