fullSlew.py

Go to the documentation of this file.

from __future__ import division, absolute_import

import sys

from coordConv import PVT

from .trapSlew import trapSlew

def fullSlew(pA, vA, pB, vB, tJerk, vMax, aMax):
    """!Compute a jerk-limited trapezoidal slew

    Inputs:
    @param[in] pA               dp      position of starting point at time t = 0 (deg)
    @param[in] vA               dp      velocity of starting point at time t = 0 (deg/sec)
    @param[in] pB               dp      position of ending point at time t = 0 (deg)
    @param[in] vB               dp      velocity of ending point at time t = 0 (deg/sec)
    @param[in] tJerk    dp      sets maximum allowed jerk and minimum slew duration (deg/sec^3)
            (see Details for more information)
    @param[in] vMax     dp      maximum allowed |velocity| (deg/sec)
    @param[in] aMax     dp      maximum allowed |acceleration| (deg/sec^2)

    @return pvtList: a list of approximately 8 PVTs; the time of the first one is 0

    @throw RuntimeError if cannot compute a slew

    @warning: |vA| must be <= vMax, |vB| must be some margin less than vMax.
        See tcc.mov.trapSlew for details.

    Details:
      tJerk sets the maximum jerk and minimum duration of slew as follows:
      - jmax = tJerk/aMax
      - minimum duration of slew = tJerk

      Here is how the subroutine works:

      First a trapezoidal slew is computed, with several special characterstics:
      - it begins a short time ("tJerk / 2") after 0
      - the duration of its constant-velocity segment is at least "tJerk"
      - the maximum possible acceleration ("aMax") is used for both constant
         acceleration segments.

      The resulting trapezoidal slew is then "rounded" to give jerk limiting,
      as follows:
      - Rounding extends for a time (tJerk / 2) (or less if required)
         symmetrically to both sides of each trapezoidal slew node.
      - The rounded segments are paths of constant jerk.
      This process does not affect the total area under the v vs. t curve,
      hence the total distance travelled remains unchanged.

      Trapezoidal slews, and possibly jerk-limiting, are discussed further
      in my orange notebook.

      Note: most of the equations used below are less algebraically
      straightforward than is possible. They have been processed to
      reduce accumulated error and limit the number of intermediate variables.
      For example, expressions such as j = a / dt are used to eliminate
      explicit reference to jerk.

    History:
    2013-12-06 ROwen    Converted from mov_fullSlew.for
    """
    def reportBug(msgStr):
        """!Write a detailed message to stderr and raise RuntimeError"""
        sys.stderr.write(msgStr + "\n")
        sys.stderr.write("fullSlew(pA=%s; pB=%s; vA=%s; vB=%s, tJerk=%s, vMax=%s; aMax=%s)\n" % \
            (pA, pB, vA, vB, tJerk, vMax, aMax))
        raise RuntimeError(msgStr)
    
    pvtList = []
        
    # compute a trapezoidal slew starting at time "tJerk / 2",
    # with the minimum duration of the constant-velocity segment = "tJerk"
    rBAi = (pB + vB * tJerk / 2) - (pA + vA * tJerk / 2)

    ts = trapSlew(rBAi=rBAi, vA=vA, vB=vB, dt2min=tJerk, vMax=vMax, aMax=aMax)

    # Compute the time and position of the end of the jerk-limited slew,
    # and the beginning and end of the constant-velocity (middle) segment.
    # Note: the constant-velocity segment may have zero duration.
    t_end = tJerk + ts.dt1 + ts.dt2 + ts.dt3
    p_end = pB + vB * t_end
    if ts.dt2 == tJerk:
        t_cv1 =          tJerk + ts.dt1
        t_cv2 = t_cv1
        p_cv1 = pA    + (tJerk + ts.dt1) * (vA + ts.vP) / 2
        p_cv2 = p_cv1
    elif ts.dt2 > tJerk:
        t_cv1 =          tJerk + ts.dt1
        t_cv2 = t_end - (tJerk + ts.dt3)
        p_cv1 = pA    + (tJerk + ts.dt1) * (vA + ts.vP) / 2
        p_cv2 = p_end - (tJerk + ts.dt3) * (ts.vP + vB) / 2
    else:
        reportBug("Bug! Trapezoidal constant-velocity segement too brief: ts.dt2=%s, tJerk=%s" % (ts.dt2, tJerk))

    # Compute time, velocity, and position for each node
    # of the jerk-limited slew, and fill the appropriate arrays.
    # Also count up the total number of nodes.

    # Beginning of the first constant jerk segment.
    pvtList.append(PVT(pA, vA, 0))

    # End of the first constant-jerk segment and beginning of the second.
    # These bracket a constant-acceleration segment; if that has zero duration,
    # then there is one less node, and the duration of the two constant-jerk
    # segments is shorter than tJerk/2.
    if ts.dt1 <= tJerk:
        pvtList.append(PVT(
            ((pA + p_cv1) / 2) + ((tJerk + ts.dt1) * (vA - ts.vP) / 6),
            (vA + ts.vP) / 2,
            t_cv1 / 2,
        ))
    else:
        pvtList.append(PVT(
            pA + tJerk * (vA + tJerk * (ts.a1 / 6)),
            vA + (tJerk / 2) * ts.a1,
            tJerk,
        ))
        pvtList.append(PVT(
            p_cv1 - tJerk * (ts.vP - tJerk * (ts.a1 / 6)),
            ts.vP - (tJerk / 2) * ts.a1,
            t_cv1 - tJerk,
        ))

    # End of the second constant-jerk segment and beginning of the third.
    # These bracket a constant-velocity segment; if that has zero duration,
    # then there is one less node (but no change in the duration of the two
    # constant-jerk segments).
    if ts.dt2 == tJerk:
        pvtList.append(PVT(
            p_cv1,
            ts.vP,
            t_cv1,
        ))
    else:
        pvtList.append(PVT(
            p_cv1,
            ts.vP,
            t_cv1,
        ))
        pvtList.append(PVT(
            p_cv2,
            ts.vP,
            t_cv2,
        ))

    # End of the third constant-jerk segment and beginning of the fourth.
    # These bracket a constant-acceleration segment; if that has zero duration,
    # then there is one less node, and the duration of the two constant-jerk
    # segments is shorter than tJerk/2.
    if ts.dt3 <= tJerk:
        pvtList.append(PVT(
            ((p_cv2 + p_end) / 2)  + ((tJerk + ts.dt3) * (ts.vP - vB) / 6),
            (ts.vP + vB) / 2,
            (t_cv2 + t_end) / 2,
        ))
    else:
        pvtList.append(PVT(
            p_cv2 + tJerk * (ts.vP + tJerk * (ts.a3 / 6)),
            ts.vP + (tJerk / 2) * ts.a3,
            t_cv2 + tJerk,
        ))
        pvtList.append(PVT(
            p_end - tJerk * (vB - tJerk * (ts.a3 / 6)),
            vB - (tJerk / 2) * ts.a3,
            t_end - tJerk,
        ))

    # End of the fourth (last) constant jerk segment.
    pvtList.append(PVT(
        p_end,
        vB,
        t_end,
    ))

    return pvtList