checkSegment.py

Go to the documentation of this file.

from __future__ import division, absolute_import

import tcc.base
from .pathSegment import PathSegment

__all__ = ["checkSegment"]

def checkSegment(pA, vA, pB, vB, dt, doPos, doVel, doAccel, doJerk, axisLim):
    """!Check a suggested path segment of constant jerk
    
    @param[in] pA      position of starting point at time t = 0
    @param[in] vA      velocity of starting point at time t = 0
    @param[in] pB      position of ending point at time t = dt
    @param[in] vB      velocity of ending point at time t = dt
    @param[in] dt      duration of motion
    @param[in] doPos   log test position?
    @param[in] doVel   log test velocity?
    @param[in] doAccel log test acceleration?
    @param[in] doJerk  log test jerk?
    @param[in] axisLim  axis limits (a tcc.base.AxisLim)
    
    @return errCode: one of the tcc.base.AxisErr_* constants defined in basics.h
    
    @todo: make this a method of PathSegment
    
    Error Conditions:
      If min and max position, max velocity, etc. cannot be computed
      for the path, errCode is set to tcc.base.AxisErr_CannotCompute.
    
      If a limit would be exceeded along the path, errCode is set appropriately.
      Note: limits are checked in this order:
         minpos, maxpos, vel, accel, jerk
      and checking terminates at the first failure.
    
    Warnings:
      Even if you request that position limits be checked, some paths
      will be accepted that have points outside the allowed region. See Details.
    
      Position and velocity limits are not checked with strict accuracy,
      but instead the limits are padded with a small fudge factor. See Details.
    
    Details:
      In handling position limits, we must allow motion from out of bounds
      towards being in bounds. Hence this routine allows paths with points
      outside the position limits in some circumstances. Specifically:
    
      If the beginning position of the path is outside the allowed limits,
      AND if the extreme pos. of the path is FURTHER outside the allowed limits,
      ONLY :is the path considered unacceptable due to position limits.
    
    History:
    2013-12-06 ROwen  Converted from mov_checkPath.for
    """
    # compute min and max position (if requested), max |velocity|,
    # initial acceleration, and constant jerk along the path
    try:
        segment = PathSegment(pA=pA, vA=vA, pB=pB, vB=vB, dt=dt, doPos=doPos)
    except RuntimeError:
        return tcc.base.AxisErr_CannotCompute

    # check the various limits, as requested
    if doPos:
        # check position limits; expand the limit to include the starting position
        # so we can move back out if we exceed a limit
        if segment.pMin < min(axisLim.minPos, pA):
            return tcc.base.AxisErr_MinPos

        if segment.pMax > max(axisLim.maxPos, pA):
            return tcc.base.AxisErr_MaxPos

    if doVel and abs(segment.vPeak) > axisLim.vel:
        return tcc.base.AxisErr_MaxVel

    if doAccel and abs(segment.aPeak) > axisLim.accel:
        return tcc.base.AxisErr_MaxAccel

    if doJerk and abs(segment.j) > axisLim.jerk: # jerk is constant along the path
        return tcc.base.AxisErr_MaxJerk

    return tcc.base.AxisErr_OK