1 from __future__
import division, absolute_import
6 __all__ = [
"PathSegment"]
9 """!A path segment of constant jerk
11 Includes a few tid-bits of extra information, including the maximum |velocity|
12 during the segment, and optionally the position extremes during the segment
23 """!Construct a PathSegment
25 @param[in] pA position of starting point at time t = 0 (deg)
26 @param[in] vA velocity of starting point at time t = 0 (deg/sec)
27 @param[in] pB position of ending point at time t = dt (deg)
28 @param[in] vB velocity of ending point at time t = dt (deg/sec)
29 @param[in] dt duration of motion (sec)
30 @param[in] doPos compute minimum and maximum position?
32 @throw RuntimeError if computations would overflow (|dt| too small or |pA-pB| too big).
34 Fields include all named arguments plus:
35 - aA acceleration of starting point at time t = 0 (deg/sec^2)
37 - pMin mimimum position during the interval (deg) (None if doPos false);
38 increased by PosOvertravel if it occurs during the move (not at an endpoint).
39 - pMax maximum position during the interval (deg) (None if doPos false);
40 decreased by PosOvertravel if it occurs during the move (not at an endpoint).
41 - vPeak maximum |velocity| during the interval (deg/sec)
42 - aPeak maximum |acceleration| during the interval (deg/sec^2)
45 2013-12-06 ROwen convert from mov_path.for
59 if dt_sq * dt_sq < coordConv.DoubleMin:
60 raise RuntimeError(
"dt=%r too small" % (dt,))
64 aA = (3.0 * vx - (2.0 * vA + vB)) * 2.0 / dt
65 j = (((vA + vB) / 2.0) - vx) * (12.0 / dt_sq)
74 if abs(aA) < abs(j * dt):
75 t_vex = max(-aA / j, 0.0)
78 vex = vA + t_vex * (aA + (t_vex / 2.0) * j)
84 self.
vPeak = max(abs(vA), abs(vB), abs(vex))
85 self.
aPeak = max(abs(aA), abs(aB))
106 if abs(vA) < abs(aA * dt):
107 t_pex_zeroj = max(-vA / aA, 0.0)
110 sqrt_arg = (aA * aA) - (2.0 * vA * j)
115 sqrt_val = math.sqrt(sqrt_arg)
116 numArr[0] = -aA - sqrt_val
117 numArr[1] = -aA + sqrt_val
118 for branch
in range(2):
119 if abs(numArr[branch]) < abs(j * dt):
120 t_pexArr[branch] = max(0.0, numArr[branch] / j)
122 t_pexArr[branch] = t_pex_zeroj
123 for branch
in range(2):
124 pexArr[branch] = pA + t_pexArr[branch] * (vA + (t_pexArr[branch] / 2.0) \
125 * (aA + (t_pexArr[branch] / 3.0) * j))
131 return "PathSegment(pA=%s, vA=%s, pB=%s, vB=%s, dt=%s, doPos=%s)" % \
132 (self.
pA, self.
vA, self.
pB, self.
vB, self.
dt, self.doPos)
A path segment of constant jerk.
def __init__
Construct a PathSegment.