def InterpolateZeroVelND(x0Vect, x1Vect, vmVect, amVect, delta=zero):
"""Interpolate a trajectory connecting two waypoints, x0Vect and x1Vect. Velocities at both
waypoints are zeros.
"""
ndof = len(x0Vect)
assert (ndof == len(x1Vect))
assert (ndof == len(vmVect))
assert (ndof == len(amVect))
# Convert all vector elements into mp.mpf (if necessary)
x0Vect_ = ConvertFloatArrayToMPF(x0Vect)
x1Vect_ = ConvertFloatArrayToMPF(x1Vect)
vmVect_ = ConvertFloatArrayToMPF(vmVect)
amVect_ = ConvertFloatArrayToMPF(amVect)
dVect = x1Vect - x0Vect
if type(delta) is not mp.mpf:
delta = mp.mpf("{:.15e}".format(delta))
vMin = inf # the tightest velocity bound
aMin = inf # the tightest acceleration bound
for i in range(ndof):
if not IsEqual(x1Vect[i], x0Vect[i]):
vMin = min(vMin, vmVect[i] / Abs(dVect[i]))
aMin = min(aMin, amVect[i] / Abs(dVect[i]))
if (not (vMin < inf and aMin < inf)):
# dVect is zero.
curvesnd = ParabolicCurvesND()
curvesnd.SetConstant(x0Vect_, 0)
return curvesnd
if delta == zero:
sdProfile = Interpolate1D(
zero, one, zero, zero, vMin,
aMin) # parabolic ramp (velocity profile sd(t))
else:
# Not handle interpolation with switch-time constraints yet
raise NotImplementedError
# Scale each DOF according to the obtained sd-profile
curves = [ParabolicCurve()
for _ in range(ndof)] # a list of (empty) parabolic curves
for sdRamp in sdProfile:
aVect = sdRamp.a * dVect
v0Vect = sdRamp.v0 * dVect
dur = sdRamp.duration
for j in range(ndof):
ramp = Ramp(v0Vect[j], aVect[j], dur, x0Vect[j])
curve = ParabolicCurve([ramp])
curves[j].Append(curve)
for (i, curve) in enumerate(curves):
curve.SetInitialValue(x0Vect[i])
curvesnd = ParabolicCurvesND(curves)
return curvesnd
def InterpolateNDFixedDuration(x0Vect_, x1Vect_, v0Vect_, v1Vect_, duration,
xminVect_, xmaxVect_, vmVect_, amVect_):
assert (duration > 0)
ndof = len(x0Vect_)
assert (ndof == len(x1Vect_))
assert (ndof == len(v0Vect_))
assert (ndof == len(v1Vect_))
assert (ndof == len(xminVect_))
assert (ndof == len(xmaxVect_))
assert (ndof == len(vmVect_))
assert (ndof == len(amVect_))
# Convert all vector elements into mp.mpf (if necessary)
x0Vect = ConvertFloatArrayToMPF(x0Vect_)
x1Vect = ConvertFloatArrayToMPF(x1Vect_)
v0Vect = ConvertFloatArrayToMPF(v0Vect_)
v1Vect = ConvertFloatArrayToMPF(v1Vect_)
xminVect = ConvertFloatArrayToMPF(xminVect_)
xmaxVect = ConvertFloatArrayToMPF(xmaxVect_)
vmVect = ConvertFloatArrayToMPF(vmVect_)
amVect = ConvertFloatArrayToMPF(amVect_)
dVect = x1Vect - x0Vect
duration = ConvertFloatToMPF(duration)
curves = []
for idof in xrange(ndof):
curve = Interpolate1DFixedDuration(x0Vect[idof], x1Vect[idof],
v0Vect[idof], v1Vect[idof],
duration, vmVect[idof],
amVect[idof])
if curve.isEmpty:
return ParabolicCurvesND()
newCurve = _ImposeJointLimitFixedDuration(curve, xminVect[idof],
xmaxVect[idof], vmVect[idof],
amVect[idof])
if newCurve.isEmpty:
return ParabolicCurvesND()
curves.append(newCurve)
return ParabolicCurvesND(curves)
def InterpolateArbitraryVelND(x0Vect_,
x1Vect_,
v0Vect_,
v1Vect_,
xminVect_,
xmaxVect_,
vmVect_,
amVect_,
delta=zero,
tryHarder=False):
"""Interpolate a trajectory connecting two waypoints, (x0Vect, v0Vect) and (x1Vect, v1Vect).
"""
ndof = len(x0Vect_)
assert (ndof == len(x1Vect_))
assert (ndof == len(v0Vect_))
assert (ndof == len(v1Vect_))
assert (ndof == len(xminVect_))
assert (ndof == len(xmaxVect_))
assert (ndof == len(vmVect_))
assert (ndof == len(amVect_))
# Convert all vector elements into mp.mpf (if necessary)
x0Vect = ConvertFloatArrayToMPF(x0Vect_)
x1Vect = ConvertFloatArrayToMPF(x1Vect_)
v0Vect = ConvertFloatArrayToMPF(v0Vect_)
v1Vect = ConvertFloatArrayToMPF(v1Vect_)
xminVect = ConvertFloatArrayToMPF(xminVect_)
xmaxVect = ConvertFloatArrayToMPF(xmaxVect_)
vmVect = ConvertFloatArrayToMPF(vmVect_)
amVect = ConvertFloatArrayToMPF(amVect_)
dVect = x1Vect - x0Vect
if type(delta) is not mp.mpf:
delta = mp.mpf("{:.15e}".format(delta))
# First independently interpolate each DOF to find out the slowest one.
curves = []
maxDuration = zero
maxIndex = 0
for i in xrange(ndof):
if delta == zero:
curve = Interpolate1D(x0Vect[i], x1Vect[i], v0Vect[i], v1Vect[i],
vmVect[i], amVect[i])
else:
raise NotImplementedError
curves.append(curve)
if curve.duration > maxDuration:
maxDuration = curve.duration
maxIndex = i
## TEMPORARY
# print "maxIndex = {0}".format(maxIndex)
curvesnd = ReinterpolateNDFixedDuration(curves, vmVect, amVect, maxIndex,
delta, tryHarder)
newCurves = []
for (i, curve) in enumerate(curvesnd.curves):
newCurve = _ImposeJointLimitFixedDuration(curve, xminVect[i],
xmaxVect[i], vmVect[i],
amVect[i])
if newCurve.isEmpty:
return ParabolicCurvesND()
newCurves.append(newCurve)
return ParabolicCurvesND(newCurves)
def ReinterpolateNDFixedDuration(curves,
vmVect,
amVect,
maxIndex,
delta=zero,
tryHarder=False):
ndof = len(curves)
assert (ndof == len(vmVect))
assert (ndof == len(amVect))
assert (maxIndex < ndof)
# Convert all vector elements into mp.mpf (if necessary)
vmVect_ = ConvertFloatArrayToMPF(vmVect)
amVect_ = ConvertFloatArrayToMPF(amVect)
newCurves = []
if not tryHarder:
newDuration = curves[maxIndex].duration
for (idof, curve) in enumerate(curves):
if idof == maxIndex:
newCurves.append(curve)
continue
stretchedCurve = _Stretch1D(curve, newDuration, vmVect[idof],
amVect[idof])
if stretchedCurve.isEmpty:
log.debug(
'ReinterpolateNDFixedDuration: dof {0} failed'.format(
idof))
return ParabolicCurvesND()
else:
newCurves.append(stretchedCurve)
assert (len(newCurves) == ndof)
return ParabolicCurvesND(newCurves)
else:
# Try harder to re-interpolate this trajectory. This is guaranteed to have 100% success rate
isPrevDurationSafe = False
newDuration = zero
for (idof, curve) in enumerate(curves):
t = _CalculateLeastUpperBoundInoperativeInterval(
curve.x0, curve.EvalPos(curve.duration), curve.v0,
curve.EvalVel(curve.duration), vmVect[idof], amVect[idof])
assert (t > zero)
if t > newDuration:
newDuration = t
assert (not (newDuration == zero))
if curves[maxIndex].duration > newDuration:
# The duration of the slowest DOF is already safe
newDuration = curves[maxIndex].duration
isPrevDurationSafe = True
for (idof, curve) in enumerate(curves):
if (isPrevDurationSafe) and (idof == maxIndex):
newCurves.append(curve)
continue
stretchedCurve = _Stretch1D(curve, newDuration, vmVect[idof],
amVect[idof])
if stretchedCurve.isEmpty:
log.debug(
'ReinterpolateNDFixedDuration: dof {0} failed even when trying harder'
.format(idof))
log.debug('x0 = {0}; x1 = {1}; v0 = {2}; v1 = {3}; vm = {4}; am = {5}; newDuration = {6}'.\
format(mp.nstr(curve.x0, n=_prec), mp.nstr(curve.EvalPos(curve.duration), n=_prec),
mp.nstr(curve.v0, n=_prec), mp.nstr(curve.EvalVel(curve.duration), n=_prec),
mp.nstr(vmVect[idof], n=_prec), mp.nstr(amVect[idof], n=_prec),
mp.nstr(newDuration, n=_prec)))
raise Exception(
'Something is wrong when calculating the least upper bound of inoperative intervals'
)
newCurves.append(stretchedCurve)
assert (len(newCurves) == ndof)
return ParabolicCurvesND(newCurves)