slew.py

Go to the documentation of this file.

from __future__ import division, absolute_import

import sys

from coordConv import PVT
import tcc.base

from .checkSegment import checkSegment
from .fullSlew import fullSlew

__all__ = ["slew"]

MaxTries = 4
MinTJerk = 0.1

def slew(pA, vA, pB, vB, tMin, axisLim):
    """!Compute a jerk-limited slew -- as a single segment, if possible, else a multi-segment path
    
    @param[in] pA     position of starting point at time t = 0 (deg)
    @param[in] vA     velocity of starting point at time t = 0 (deg/sec)
    @param[in] pB     position of ending point at time t = 0 (deg)
    @param[in] vB     velocity of ending point at time t = 0 (deg/sec)
    @param[in] tMin   minimum duration of slew (sec); if < 0 then 0 is silently used
    @param[in] axisLim  axis limits (a tcc.base.AxisLim); position limits are ignored
    
    @return pvtList: a list of at least 2 PVTs; the time of the first one is 0
    
    @throw RuntimError if:
    - The jerk limit is 0 (or so small that division overflows)
    - The slew could not be computed

    History:
    2013-12-06 ROwen    Converted from mov_Slew.for
    """
    def formatArgs():
        return "tcc.mov.slew(pA=%r, vA=%r, pB=%r, vB=%r, tMin=%r, axisLim=%s)" % (pA, vA, pB, vB, tMin, axisLim)

    def reportBug(msgStr):
        """!Write a message to stderr that includes outputs, then raise RuntimeError
        """
        sys.stderr.write("%s failed:\n%s\n" % (formatArgs(), msgStr))
        raise RuntimeError(msgStr)

    try:
        t_jerk = max(axisLim.accel / axisLim.jerk, MinTJerk)
    except ZeroDivisionError:
        raise RuntimeError("Cannot compute slew; jerk limit is 0 (or nearly so)")

    try:
        # Try longer and longer 2-node paths to see if any are acceptable.
        # The intervals tested must be at least tMin, but because tMin may
        # be very small or zero, I use the greater of tMin and t_jerk;
        # t_jerk is a physically sensible scale factor for slewing.
        pvtList = []
        tryTime = 0.0
        tryInterval = max(tMin, t_jerk)
        for twoNodeIter in range(MaxTries):
            tryTime += tryInterval
            pB_end = pB + (vB * tryTime)
            # check vel, accel, jerk, but not pos (we don't know the pos limits)

            twoNodeErrCode = checkSegment(
                pA=pA, vA=vA, pB=pB_end, vB=vB, dt=tryTime,
                doPos=False, doVel=True, doAccel=True, doJerk=True, axisLim=axisLim,
            )
            if twoNodeErrCode == tcc.base.AxisErr_OK:
                pvtList = (
                    PVT(pA,     vA,       0),
                    PVT(pB_end, vB, tryTime),
                )
                break

        # if a two-node path is acceptable, use it
        if not pvtList:
            # no two-node path found; compute a full path
            pvtList = fullSlew(pA, vA, pB, vB, t_jerk, axisLim.vel, axisLim.accel)

            # if slew too short, increase t_jerk and compute again
            # full slews can be almost as short as 2*t_jerk, hence to gurantee
            # that full slew is long enough, t_jerk must be >= tMin / 2
            if pvtList[-1].t < tMin:
                t_jerk = tMin / 2.0
                pvtList = fullSlew (pA, vA, pB, vB, t_jerk, axisLim.vel, axisLim.accel)
    except Exception as e:
        sys.stderr.write("%s failed:\n%s" % (formatArgs(), e))
        raise

    if pvtList[-1].t < tMin:
        reportBug('Bug! Computed slew too short: pvtList[-1].t=%s < tMin=%s' % (pvtList[-1].t, tMin))
        
    return pvtList