pathSegment.py

Go to the documentation of this file.

from __future__ import division, absolute_import

import math
import coordConv

__all__ = ["PathSegment"]

class PathSegment(object):
    """!A path segment of constant jerk
    
    Includes a few tid-bits of extra information, including the maximum |velocity|
    during the segment, and optionally the position extremes during the segment
    """
    PosOvertravel = 1e-4 # amount to increase pMin or decrease pMax if that limit occurs partway through
        # a move (rather than at an endpoint). This is to make sure one can move away from a limit.
        # I think the main concern is roundoff error leading to pMin or pMax being slightly off,
        # though I'm not positive. Certainly the checkSegment already includes the starting point
        # when checking limits, which ought to suffice.     
        # In the old TCC it was 0.01 degrees, but I doubt there was justification for that.
        # I'll assume roundoff error is the only issue and so use a much smaller value.

    def __init__(self, pA, vA, pB, vB, dt, doPos):
        """!Construct a PathSegment

        @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 = dt (deg)
        @param[in] vB       velocity of ending point at time t = dt (deg/sec)
        @param[in] dt       duration of motion (sec)
        @param[in] doPos    compute minimum and maximum position?

        @throw RuntimeError if computations would overflow (|dt| too small or |pA-pB| too big).
        
        Fields include all named arguments plus:
        - aA    acceleration of starting point at time t = 0 (deg/sec^2)
        - j     jerk (deg/sec^3)
        - pMin  mimimum position during the interval (deg) (None if doPos false);
                increased by PosOvertravel if it occurs during the move (not at an endpoint).
        - pMax  maximum position during the interval (deg) (None if doPos false);
                decreased by PosOvertravel if it occurs during the move (not at an endpoint).
        - vPeak maximum |velocity| during the interval (deg/sec)
        - aPeak maximum |acceleration| during the interval (deg/sec^2)

        History:
        2013-12-06  ROwen   convert from mov_path.for
        """
        self.pA = pA
        self.vA = vA
        self.pB = pB
        self.vB = vB
        self.dt = dt

        dt_sq = dt * dt

        # crude test for overflow of vx, aA, and j;
        # assumes |numerator| < sqrt(bignum),
        # tests |denominator| > sqrt(smallnum)
        if abs(dt) < 1.0:
            if dt_sq * dt_sq < coordConv.DoubleMin:
                raise RuntimeError("dt=%r too small" % (dt,))

        # compute aA, j and aB
        vx = (pB - pA) / dt
        aA = (3.0 * vx - (2.0 * vA + vB)) * 2.0 / dt
        j = (((vA + vB) / 2.0) - vx) * (12.0 / dt_sq)
        aB = aA + (j * dt)

        #+
        # compute maximum |velocity| (vPeak);
        # this may occur at the endpoints or at time t_vex = - aA/j
        #-
        # compute t_vex and vex = v(t_vex);
        # if t_vex is out of the range [0, dt), set t_vex = 0, so that vex = vA
        if abs(aA) < abs(j * dt):
            t_vex = max(-aA / j, 0.0)
        else:
            t_vex = 0.0
        vex = vA + t_vex * (aA + (t_vex / 2.0) * j)

        self.aA = aA
        self.j = j
        self.pMin = None
        self.pMax = None
        self.vPeak = max(abs(vA), abs(vB), abs(vex))
        self.aPeak = max(abs(aA), abs(aB))

        #+
        # If desired, compute minimum and maximum position (pMin and pMax)
        #-
        # pMin and pMax may occur at the endpoints or at times t_pex1 or t_pex2
        # (the two solutions to the quadratic equation v(t) = 0).
        # Note that lim (j->0) t_pexArr = - vA / aA, yet the equation used below
        # is ill-behaved at small j. Also, it's hard to distinguish the cases
        # t_pexArr out of the range [0,dt], and t_pexArr unevaluatable.
        # Rather than try, I simply use - vA / aA whenever the standard
        # equation would not give me a reasonable answer
        # (hence either t_pexArr = an endpoint, or =[0] - vA / aA;
        # in the former case, - vA / aA simply won't give a maximal position).
        if doPos:
            t_pexArr = [0]*2
            numArr = [0]*2
            pexArr = [0]*2

            # compute the two times t_pexArr, and positions pexArr = p(t_pexArr);
            # if a t_pexArr is out of range [0, dt), set it to 0 (so its pexArr = pA)
            if abs(vA) < abs(aA * dt):
                t_pex_zeroj = max(-vA / aA, 0.0)
            else:
                t_pex_zeroj = 0.0
            sqrt_arg = (aA * aA) - (2.0 * vA * j)
            if sqrt_arg < 0.0:
                t_pexArr[0] = 0.0
                t_pexArr[1] = 0.0
            else:
                sqrt_val = math.sqrt(sqrt_arg)
                numArr[0] = -aA - sqrt_val
                numArr[1] = -aA + sqrt_val
                for branch in range(2):
                    if abs(numArr[branch]) < abs(j * dt):
                        t_pexArr[branch] = max(0.0, numArr[branch] / j)
                    else:
                        t_pexArr[branch] = t_pex_zeroj
            for branch in range(2):
                pexArr[branch] = pA + t_pexArr[branch] * (vA + (t_pexArr[branch] / 2.0) \
                    * (aA + (t_pexArr[branch] / 3.0) * j))

            self.pMin = min(pA, pB, pexArr[0] + self.PosOvertravel, pexArr[1] + self.PosOvertravel)
            self.pMax = max(pA, pB, pexArr[0] - self.PosOvertravel, pexArr[1] - self.PosOvertravel)

    def __repr__(self):
        return "PathSegment(pA=%s, vA=%s, pB=%s, vB=%s, dt=%s, doPos=%s)" % \
            (self.pA, self.vA, self.pB, self.vB, self.dt, self.doPos)