trapSlew.py

Go to the documentation of this file.

from __future__ import division, absolute_import

import math
import sys

from coordConv import DoubleMax

__all__ = ["trapSlew"]

Fudge = 1.05  # fudge factor to avoid borderline cases

class TrapSlewData(object):
    """!Data returned by trapSlew. Fields match inputs to constructor.
    """
    def __init__(self, dt1, dt2, dt3, vP, a1, a3, slewType):
        """!Construct a TrapSlewData
        
        @param[in] dt1  duration of segment 1 (first constant acceleration) (sec)
        @param[in] dt2  duration of segment 2 (constant velocity) (sec)
        @param[in] dt3  duration of segment 3 (last constant acceleration) (sec)
        @param[in] vP   velocity of segment 2 (deg/sec)
        @param[in] a1   acceleration of segment 1 (deg/sec^2)
        @param[in] a3   acceleration of segment 3 (deg/sec^2)
        @param[in] slewType type of slew; one of:
                        0: null slew: vB = vA and rBAi = 0:
                            a1 = a3 = 0
                        1: sign(vB - vA) = sign(rBAi), or one of vBA or rBAi = 0:
                            sign(a1) = sign(rBAi), |a1| = aMax, a3 = - a1
                        2: sign(vB - vA) = - sign(rBAi), and a solution exists such that:
                            sign(a1) = sign(rBAi), |a1| = aMax, a3 = - a1
                        3: sign(vB - vA) = - sign(rBAi), and a solution exists such that:
                            sign(a1) = - sign(rBAi), |a1| = aMax, a3 = - a1
                        4: same as type 3 but |a1| reduced as required so a solution exists
        """
        self.dt1 = float(dt1)
        self.dt2 = float(dt2)
        self.dt3 = float(dt3)
        self.vP = float(vP)
        self.a1 = float(a1)
        self.a3 = float(a3)
        self.slewType = int(slewType)
    
    def __repr__(self):
        return "TrapSlewData(dt1=%s, dt2=%s, dt3=%s, vP=%s, a1=%s, a3=%s, slewType=%s)" % \
            (self.dt1, self.dt2, self.dt3, self.vP, self.a1, self.a3, self.slewType)


def trapSlew(rBAi, vA, vB, dt2min, vMax, aMax):
    """!Compute a trapezoidal slew.
    
    You may specify a minimum duration for the constant-velocity segment, which is useful for "rounding the
    corners" of the slew to make it jerk-limited (see mov.fullSlew).

    @param[in] rBAi     distance between "A" and "B" at time t = 0 (deg)
    @param[in] vA       velocity of "A" (deg/sec)
    @param[in] vB       velocity of "B" (deg/sec)
    @param[in] dt2min   minimum duration of segment 2 (constant velocity) (sec)
    @param[in] vMax     maximum allowed velocity (deg/sec)
    @param[in] aMax     maximum allowed acceleration (deg/sec^2)

    @return a TrapSlewData

    @throw RuntimeError if:
    * dt2min < 0
    * vMax <= 0
    * aMax <= 0
    * |vB| > vMax / Fudge

    Dies if vMax or aMax are so small as to cause under- or over-flow.

    Details:
      The magic number "Fudge" is used to avoid borderline cases.
      It is set below, and should be a number a bit bigger than one.

      How it works:
      The slew begins by tracking object A, and ends by tracking object B.
      Both objects are assumed to be moving at constant velocity.

      A trapezoidal slew has three segments, two of constant acceleration
      separated by a constant velocity segment. It is called "trapezoidal"
      because that is the shape of the v vs. t curve.

      Here are the initial velocity and constant acceleration for each segment,
      and the duration of that segment, in the notation of this subroutine:

      segment   v   a   duration
         1      vA  a1  dt1
         2      vP  0   dt2
         3      vP  a2  dt3


      The slew numbering and notation used in this subroutine are quite different
      than those used in the math notebook. Significant changes include:
      dt2 = delta-t3 in notebook
      slew type 0 is not mentioned in the notebook
      slew type 1 = notebook type 0
      slew type 2 = notebook type 1
      slew type 3, 4 = notebook type 3
      note that the notebook type 2 slew (the only "reversed" slew)
      is NOT USED by this subroutine, because it is not needed, and it saves
      the bother of implementing the reversed slew equations; instead,
      a type 3 or 4 slew (this subr.) is used with reduced acceleration.

    References:
      "Control of the Apache Point 3.5m Telescope: Slewing", R. Owen, 1990, unpub

      TCC Math Notebook, section on slewing (warning: different notation)

    History:
    2013-12-06 ROwen    Converted from mov_TrapSlew.for
    """
    def reportBug(msgStr):
        """!Write a message to stderr that includes outputs, then raise RuntimeError
        """
        sys.stderr.write("%s\ntrapSlew(rBai=%s, vA=%s, vB=%s, dt2min=%s, vMax=%s, aMax=%s)\n" % \
            (msgStr, rBAi, vA, vB, dt2min, vMax, aMax))
        raise RuntimeError(msgStr)

    if dt2min < 0.0:
        raise RuntimeError("dt2min=%s < 0" % (dt2min,))
    if vMax <= 0.0:
        raise RuntimeError("vMax=%s < 0" % (vMax,))
    if aMax <= 0.0:
        raise RuntimeError("aMax=%s < 0" % (aMax,))

    # check velocities; errors are: |vB| Fudge > vMax,
    # |vA| Fudge > vMax (can lead to dt1 < 0 for type 2 slews)
    if abs(vA) > vMax * Fudge:
        raise RuntimeError("Telescope is moving too fast (|%0.4f| > %s * %s). Stop the telescope, then try your slew again." 
            % (vA, Fudge, vMax))
    if abs(vB) * Fudge > vMax:
        raise RuntimeError("Target is moving too fast (|%0.4f| * %s > %s; telescope cannot acquire it." \
            % (vB, Fudge, vMax))

    #+
    # compute vBA, half_vBAsq, sign_rBAi and sign_vBA
    # and handle null slews (rBAi and vBA both zero)
    #-
    vBA = vB - vA
    half_vBAsq = 0.5 * vBA * vBA
    if (rBAi != 0.0) and (vBA != 0.0):
        sign_rBAi = math.copysign(1.0, rBAi)
        sign_vBA  = math.copysign(1.0, vBA)
    elif rBAi != 0.0:
        sign_rBAi = math.copysign(1.0, rBAi)
        sign_vBA  = sign_rBAi
    elif vBA  != 0.0:
        sign_vBA  = math.copysign(1.0, vBA)
        sign_rBAi = sign_vBA
    else:
        return TrapSlewData(
            dt1 = 0,
            dt2 = dt2min,
            dt3 = 0,
            vP = vA,
            a1 = 0,
            a3 = 0,
            slewType = 0,
        )

    #+
    # compute slewType, a1 and a3
    #-
    # if sign(rBAi) = sign(vBA), slew is type 1
    # a solution is sure because dt3 has no upper limit over range of soln
    if sign_rBAi == sign_vBA:
        slewType = 1
        a1  = sign_rBAi * aMax

    # else sign(rBAi) = - sign(vBA) so we use type 2, 3 or 4 slew...

    # a type 2 slew has a maximum dt2 dependent on initial conditions;
    # the biggest dt2 occurs at largest |a|, |a| = aMax,
    # and smallest |vPB|, |vPB| = |vBA|
    # so test at that point to see if solutions exist with dt2 > dt2min
    elif abs(vA) * Fudge < vMax and (dt2min * aMax * abs(vBA)) <= ((aMax * abs(rBAi)) - half_vBAsq):
        slewType = 2
        a1  = sign_rBAi * aMax

    # a type 3 slew only exists if aMax is small enough
    elif (aMax * abs(rBAi) * Fudge) <= half_vBAsq:
        slewType = 3
        a1 = - sign_rBAi * aMax

    # a type 4 slew requires reducing accel. to obtain a solution
    else:
        slewType = 4
        # since the equation for a1 is sure to give |a1| < aMax
        # (otherwise slew would have been type 3)
        # the equation is guranteed to not overflow
        a1 = - half_vBAsq / (Fudge * rBAi)
    a3 = - a1

    # make sure velocity / acceleration divisions will not overflow;
    # this is especially important for slew type 4 because acceleration
    # gets reduced, but could also catch stupid aMax or vMax inputs
    max_vdiff = vMax + abs(vA) + abs(vB)
    if max_vdiff >= min(abs(a1), 1.0) * DoubleMax:
        raise reportBug('Computed slew time is ridiculous')

    #+
    # compute dt2 and vP
    #-
    # first assume that dt2 = dt2min and compute vP;
    # if resulting vP is too big, reduce it to maximum allowed
    # and compute corresponding increased dt2
    dt2 = dt2min
    vPB_temp = (0.5 * a1 * dt2)**2 + half_vBAsq + a1 * rBAi
    if vPB_temp < 0.0:
        raise RuntimeError('Bug! Tried to compute square root of negative value')
    vPB = math.copysign(math.sqrt(vPB_temp), a1) - (0.5 * a1 * dt2)
    vP = vPB + vB
    if abs(vP) > vMax:
        # |vP| is too big, and so must be reduced.
        # Note that |vB| < vMax / Fudge (as tested far above),
        # so |vP| is guaranteed to be reducible to vMax without causing
        # vPB to approach zero (much less change sign).
        # The division velocity / acceleration was proved safe above.
        # Thus dt2 may be computed without overflow.
        vP = math.copysign(vMax, vP)
        vPB = vP - vB
        dt2 = (rBAi + ((half_vBAsq - (vPB * vPB)) / a1)) / vPB
    #+
    # compute dt1 and dt3
    #-
    # the following divisions were proved safe from overflow above,
    # just after computing a1 and a3
    vPA =   vPB + vBA
    dt1 =   vPA / a1
    dt3 = - vPB / a3

    # sanity checks
    if (dt1 < 0.0) or (dt2 < 0.0) or (dt3 < 0.0):
        reportBug('Bug! Computed negative duration for one or more segments')
    if abs(vP) > vMax:
        reportBug('Bug! Computed velocity greater than max velocity')
    if abs(a1) > aMax:
        reportBug('Bug! Computed acceleration greater than max acceleration')

    return TrapSlewData(
        dt1 = dt1,
        dt2 = dt2,
        dt3 = dt3,
        vP = vP,
        a1 = a1,
        a3 = a3,
        slewType = slewType,
    )