def _Interpolate1DNoVelocityLimit(x0, x1, v0, v1, am):
# Check types
if type(x0) is not mp.mpf:
x0 = mp.mpf("{:.15e}".format(x0))
if type(x1) is not mp.mpf:
x1 = mp.mpf("{:.15e}".format(x1))
if type(v0) is not mp.mpf:
v0 = mp.mpf("{:.15e}".format(v0))
if type(v1) is not mp.mpf:
v1 = mp.mpf("{:.15e}".format(v1))
if type(am) is not mp.mpf:
am = mp.mpf("{:.15e}".format(am))
# Check inputs
assert(am > zero)
# Check for an appropriate acceleration direction of the first ramp
d = Sub(x1, x0)
dv = Sub(v1, v0)
difVSqr = Sub(v1**2, v0**2)
if Abs(dv) < epsilon:
if Abs(d) < epsilon:
# Stationary ramp
ramp0 = Ramp(zero, zero, zero, x0)
return ParabolicCurve([ramp0])
else:
dStraight = zero
else:
dStraight = mp.fdiv(difVSqr, Prod([2, mp.sign(dv), am]))
if IsEqual(d, dStraight):
# With the given distance, v0 and v1 can be directly connected using max/min
# acceleration. Here the resulting profile has only one ramp.
a0 = mp.sign(dv) * am
ramp0 = Ramp(v0, a0, mp.fdiv(dv, a0), x0)
return ParabolicCurve([ramp0])
sumVSqr = Add(v0**2, v1**2)
sigma = mp.sign(Sub(d, dStraight))
a0 = sigma * am # acceleration of the first ramp
vp = sigma * mp.sqrt(Add(Mul(pointfive, sumVSqr), Mul(a0, d)))
t0 = mp.fdiv(Sub(vp, v0), a0)
t1 = mp.fdiv(Sub(vp, v1), a0)
ramp0 = Ramp(v0, a0, t0, x0)
assert(IsEqual(ramp0.v1, vp)) # check soundness
ramp1 = Ramp(vp, Neg(a0), t1)
curve = ParabolicCurve([ramp0, ramp1])
assert(IsEqual(curve.d, d)) # check soundness
return curve
def _ImposeVelocityLimit(curve, vm):
"""_ImposeVelocityLimit imposes the given velocity limit to the ParabolicCurve. In case the velocity
limit cannot be satisfied, this function will return an empty ParabolicCurve.
"""
# Check types
if type(vm) is not mp.mpf:
vm = mp.mpf("{:.15e}".format(vm))
# Check inputs
assert (vm > zero)
assert (len(curve) == 2)
assert (Add(curve[0].a, curve[1].a) == zero)
if Sub(Abs(curve[0].v0), vm) > epsilon:
# Initial velocity violates the constraint
return ParabolicCurve()
if Sub(Abs(curve[1].v1), vm) > epsilon:
# Final velocity violates the constraint
return ParabolicCurve()
vp = curve[1].v0
if Abs(vp) <= vm:
# Velocity limit is not violated
return curve
# h = Sub(Abs(vp), vm)
# t = mp.fdiv(h, Abs(curve[0].a))
ramp0, ramp1 = curve
h = Sub(Abs(vp), vm)
t = mp.fdiv(h, Abs(ramp0.a))
# import IPython; IPython.embed()
ramps = []
if IsEqual(Abs(ramp0.v0), vm) and (mp.sign(ramp0.v0) == mp.sign(vp)):
assert (IsEqual(ramp0.duration, t)) # check soundness
else:
newRamp0 = Ramp(ramp0.v0, ramp0.a, Sub(ramp0.duration, t), ramp0.x0)
ramps.append(newRamp0)
nom = h**2
denom = Mul(Abs(curve[0].a), vm)
newRamp1 = Ramp(Mul(mp.sign(vp), vm), zero,
Sum([t, t, mp.fdiv(nom, denom)]), curve.x0)
ramps.append(newRamp1)
if IsEqual(Abs(ramp1.v1), vm) and (mp.sign(ramp1.v1) == mp.sign(vp)):
assert (IsEqual(ramp1.duration, t)) # check soundness
else:
newRamp2 = Ramp(Mul(mp.sign(vp), vm), ramp1.a, Sub(ramp1.duration, t))
ramps.append(newRamp2)
return ParabolicCurve(ramps)
def _ImposeVelocityLimit(curve, vm):
"""_ImposeVelocityLimit imposes the given velocity limit to the ParabolicCurve. In case the velocity
limit cannot be satisfied, this function will return an empty ParabolicCurve.
"""
# Check types
if type(vm) is not mp.mpf:
vm = mp.mpf("{:.15e}".format(vm))
# Check inputs
assert(vm > zero)
assert(len(curve) == 2)
assert(Add(curve[0].a, curve[1].a) == zero)
if Sub(Abs(curve[0].v0), vm) > epsilon:
# Initial velocity violates the constraint
return ParabolicCurve()
if Sub(Abs(curve[1].v1), vm) > epsilon:
# Final velocity violates the constraint
return ParabolicCurve()
vp = curve[1].v0
if Abs(vp) <= vm:
# Velocity limit is not violated
return curve
# h = Sub(Abs(vp), vm)
# t = mp.fdiv(h, Abs(curve[0].a))
ramp0, ramp1 = curve
h = Sub(Abs(vp), vm)
t = mp.fdiv(h, Abs(ramp0.a))
# import IPython; IPython.embed()
ramps = []
if IsEqual(Abs(ramp0.v0), vm) and (mp.sign(ramp0.v0) == mp.sign(vp)):
assert(IsEqual(ramp0.duration, t)) # check soundness
else:
newRamp0 = Ramp(ramp0.v0, ramp0.a, Sub(ramp0.duration, t), ramp0.x0)
ramps.append(newRamp0)
nom = h**2
denom = Mul(Abs(curve[0].a), vm)
newRamp1 = Ramp(Mul(mp.sign(vp), vm), zero, Sum([t, t, mp.fdiv(nom, denom)]), curve.x0)
ramps.append(newRamp1)
if IsEqual(Abs(ramp1.v1), vm) and (mp.sign(ramp1.v1) == mp.sign(vp)):
assert(IsEqual(ramp1.duration, t)) # check soundness
else:
newRamp2 = Ramp(Mul(mp.sign(vp), vm), ramp1.a, Sub(ramp1.duration, t))
ramps.append(newRamp2)
return ParabolicCurve(ramps)
def Interpolate1DFixedDuration(x0, x1, v0, v1, newDuration, vm, am):
x0 = ConvertFloatToMPF(x0)
x1 = ConvertFloatToMPF(x1)
v0 = ConvertFloatToMPF(v0)
v1 = ConvertFloatToMPF(v1)
vm = ConvertFloatToMPF(vm)
am = ConvertFloatToMPF(am)
newDuration = ConvertFloatToMPF(newDuration)
log.debug("\nx0 = {0}; x1 = {1}; v0 = {2}; v1 = {3}; vm = {4}; am = {5}; newDuration = {6}".\
format(mp.nstr(x0, n=_prec), mp.nstr(x1, n=_prec), mp.nstr(v0, n=_prec), mp.nstr(v1, n=_prec),
mp.nstr(vm, n=_prec), mp.nstr(am, n=_prec), mp.nstr(newDuration, n=_prec)))
# Check inputs
assert (vm > zero)
assert (am > zero)
if (newDuration < -epsilon):
return ParabolicCurve()
if (newDuration <= epsilon):
# Check if this is a stationary trajectory
if (FuzzyEquals(x0, x1, epsilon) and FuzzyEquals(v0, v1, epsilon)):
ramp0 = Ramp(v0, 0, 0, x0)
newCurve = ParabolicCurve(ramp0)
return newCurve
else:
# newDuration is too short to any movement to be made
return ParabolicCurve()
d = Sub(x1, x0)
# First assume no velocity bound -> re-interpolated trajectory will have only two ramps.
# Solve for a0 and a1 (the acceleration of the first and the last ramps).
# a0 = A + B/t0
# a1 = A + B/(t - t0)
# where t is the (new) total duration, t0 is the (new) duration of the first ramp, and
# A = (v1 - v0)/t
# B = (2d/t) - (v0 + v1).
newDurInverse = mp.fdiv(one, newDuration)
A = Mul(Sub(v1, v0), newDurInverse)
B = Sub(Prod([mp.mpf('2'), d, newDurInverse]), Add(v0, v1))
interval0 = iv.mpf([zero, newDuration]) # initial interval for t0
# Now consider the interval(s) computed from a0's constraints
sum1 = Neg(Add(am, A))
sum2 = Sub(am, A)
C = mp.fdiv(B, sum1)
D = mp.fdiv(B, sum2)
log.debug("\nA = {0}; \nB = {1}; \nC = {2}; \nD = {3}; \nsum1 = {4}; \nsum2 = {5};".\
format(mp.nstr(A, n=_prec), mp.nstr(B, n=_prec), mp.nstr(C, n=_prec), mp.nstr(D, n=_prec),
mp.nstr(sum1, n=_prec), mp.nstr(sum2, n=_prec)))
if (sum1 > zero):
# This implied that the duration is too short
log.debug("the given duration ({0}) is too short.".format(newDuration))
return ParabolicCurve()
if (sum2 < zero):
# This implied that the duration is too short
log.debug("the given duration ({0}) is too short.".format(newDuration))
return ParabolicCurve()
if IsEqual(sum1, zero):
raise NotImplementedError # not yet considered
elif sum1 > epsilon:
log.debug("sum1 > 0. This implies that newDuration is too short.")
return ParabolicCurve()
else:
interval1 = iv.mpf([C, inf])
if IsEqual(sum2, zero):
raise NotImplementedError # not yet considered
elif sum2 > epsilon:
interval2 = iv.mpf([D, inf])
else:
log.debug("sum2 < 0. This implies that newDuration is too short.")
return ParabolicCurve()
if Sub(interval2.a, interval1.b) > epsilon or Sub(interval1.a,
interval2.b) > epsilon:
# interval1 and interval2 do not intersect each other
return ParabolicCurve()
# interval3 = interval1 \cap interval2 : valid interval for t0 computed from a0's constraints
interval3 = iv.mpf(
[max(interval1.a, interval2.a),
min(interval1.b, interval2.b)])
# Now consider the interval(s) computed from a1's constraints
if IsEqual(sum1, zero):
raise NotImplementedError # not yet considered
elif sum1 > epsilon:
log.debug("sum1 > 0. This implies that newDuration is too short.")
return ParabolicCurve()
else:
interval4 = iv.mpf([Neg(inf), Add(C, newDuration)])
if IsEqual(sum2, zero):
raise NotImplementedError # not yet considered
elif sum2 > epsilon:
interval5 = iv.mpf([Neg(inf), Add(D, newDuration)])
else:
log.debug("sum2 < 0. This implies that newDuration is too short.")
return ParabolicCurve()
if Sub(interval5.a, interval4.b) > epsilon or Sub(interval4.a,
interval5.b) > epsilon:
log.debug("interval4 and interval5 do not intersect each other")
return ParabolicCurve()
# interval6 = interval4 \cap interval5 : valid interval for t0 computed from a1's constraints
interval6 = iv.mpf(
[max(interval4.a, interval5.a),
min(interval4.b, interval5.b)])
# import IPython; IPython.embed()
if Sub(interval3.a, interval6.b) > epsilon or Sub(interval6.a,
interval3.b) > epsilon:
log.debug("interval3 and interval6 do not intersect each other")
return ParabolicCurve()
# interval7 = interval3 \cap interval6
interval7 = iv.mpf(
[max(interval3.a, interval6.a),
min(interval3.b, interval6.b)])
if Sub(interval0.a, interval7.b) > epsilon or Sub(interval7.a,
interval0.b) > epsilon:
log.debug("interval0 and interval7 do not intersect each other")
return ParabolicCurve()
# interval8 = interval0 \cap interval7 : valid interval of t0 when considering all constraints (from a0 and a1)
interval8 = iv.mpf(
[max(interval0.a, interval7.a),
min(interval0.b, interval7.b)])
# import IPython; IPython.embed()
# We choose the value t0 (the duration of the first ramp) by selecting the mid point of the
# valid interval of t0.
t0 = _SolveForT0(A, B, newDuration, interval8)
if t0 is None:
# The fancy procedure fails. Now consider no optimization whatsoever.
# TODO: Figure out why solving fails.
t0 = mp.convert(interval8.mid) # select the midpoint
# return ParabolicCurve()
t1 = Sub(newDuration, t0)
a0 = Add(A, Mul(mp.fdiv(one, t0), B))
if (Abs(t1) < epsilon):
a1 = zero
else:
a1 = Add(A, Mul(mp.fdiv(one, Neg(t1)), B))
assert (Sub(Abs(a0), am) < epsilon
) # check if a0 is really below the bound
assert (Sub(Abs(a1), am) < epsilon
) # check if a1 is really below the bound
# import IPython; IPython.embed()
# Check if the velocity bound is violated
vp = Add(v0, Mul(a0, t0))
if Abs(vp) > vm:
vmnew = Mul(mp.sign(vp), vm)
D2 = Prod([
pointfive,
Sqr(Sub(vp, vmnew)),
Sub(mp.fdiv(one, a0), mp.fdiv(one, a1))
])
# print "D2",
# mp.nprint(D2, n=_prec)
# print "vmnew",
# mp.nprint(vmnew, n=_prec)
A2 = Sqr(Sub(vmnew, v0))
B2 = Neg(Sqr(Sub(vmnew, v1)))
t0trimmed = mp.fdiv(Sub(vmnew, v0), a0)
t1trimmed = mp.fdiv(Sub(v1, vmnew), a1)
C2 = Sum([
Mul(t0trimmed, Sub(vmnew, v0)),
Mul(t1trimmed, Sub(vmnew, v1)),
Mul(mp.mpf('-2'), D2)
])
log.debug("\nA2 = {0}; \nB2 = {1}; \nC2 = {2}; \nD2 = {3};".format(
mp.nstr(A2, n=_prec), mp.nstr(B2, n=_prec), mp.nstr(C2, n=_prec),
mp.nstr(D2, n=_prec)))
temp = Prod([A2, B2, B2])
initguess = mp.sign(temp) * (Abs(temp)**(1. / 3.))
root = mp.findroot(lambda x: Sub(Prod([x, x, x]), temp), x0=initguess)
# import IPython; IPython.embed()
log.debug("root = {0}".format(mp.nstr(root, n=_prec)))
a0new = mp.fdiv(Add(A2, root), C2)
if (Abs(a0new) > Add(am, epsilon)):
if FuzzyZero(Sub(Mul(C2, a0new), A2), epsilon):
# The computed a0new is exceeding the bound and its corresponding a1new is
# zero. Therefore, there is no other way to fix this. This is probably because the
# given newDuration is less than the minimum duration (x0, x1, v0, v1, vm, am) can
# get.
log.debug(
"abs(a0new) > am and a1new = 0; Cannot fix this case. This happens probably because the given newDuration is too short."
)
return ParabolicCurve()
a0new = Mul(mp.sign(a0new), am)
if (Abs(a0new) < epsilon):
a1new = mp.fdiv(B2, C2)
if (Abs(a1new) > Add(am, epsilon)):
# Similar to the case above
log.debug(
"a0new = 0 and abs(a1new) > am; Cannot fix this case. This happens probably because the given newDuration is too short."
)
return ParabolicCurve()
else:
if FuzzyZero(Sub(Mul(C2, a0new), A2), epsilon):
# import IPython; IPython.embed()
a1new = 0
else:
a1new = Mul(mp.fdiv(B2, C2),
Add(one, mp.fdiv(A2, Sub(Mul(C2, a0new), A2))))
if (Abs(a1new) > Add(am, epsilon)):
a1new = Mul(mp.sign(a1new), am)
a0new = Mul(mp.fdiv(A2, C2),
Add(one, mp.fdiv(B2, Sub(Mul(C2, a1new), B2))))
if (Abs(a0new) > Add(am, epsilon)) or (Abs(a1new) > Add(am, epsilon)):
log.warn("Cannot fix acceleration bounds violation")
return ParabolicCurve()
log.debug(
"\na0 = {0}; \na0new = {1}; \na1 = {2}; \na1new = {3};".format(
mp.nstr(a0, n=_prec), mp.nstr(a0new, n=_prec),
mp.nstr(a1, n=_prec), mp.nstr(a1new, n=_prec)))
if (Abs(a0new) < epsilon) and (Abs(a1new) < epsilon):
log.warn("Both accelerations are zero. Should we allow this case?")
return ParabolicCurve()
if (Abs(a0new) < epsilon):
# This is likely because v0 is at the velocity bound
t1new = mp.fdiv(Sub(v1, vmnew), a1new)
assert (t1new > 0)
ramp2 = Ramp(v0, a1new, t1new)
t0new = Sub(newDuration, t1new)
assert (t0new > 0)
ramp1 = Ramp(v0, zero, t0new, x0)
newCurve = ParabolicCurve([ramp1, ramp2])
return newCurve
elif (Abs(a1new) < epsilon):
t0new = mp.fdiv(Sub(vmnew, v0), a0new)
assert (t0new > 0)
ramp1 = Ramp(v0, a0new, t0new, x0)
t1new = Sub(newDuration, t0new)
assert (t1new > 0)
ramp2 = Ramp(ramp1.v1, zero, t1new)
newCurve = ParabolicCurve([ramp1, ramp2])
return newCurve
else:
# No problem with those new accelerations
# import IPython; IPython.embed()
t0new = mp.fdiv(Sub(vmnew, v0), a0new)
if (t0new < 0):
log.debug(
"t0new < 0. The given newDuration not achievable with the given bounds"
)
return ParabolicCurve()
t1new = mp.fdiv(Sub(v1, vmnew), a1new)
if (t1new < 0):
log.debug(
"t1new < 0. The given newDuration not achievable with the given bounds"
)
return ParabolicCurve()
if (Add(t0new, t1new) > newDuration):
# Final fix. Since we give more weight to acceleration bounds, we make the velocity
# bound saturated. Therefore, we set vp to vmnew.
# import IPython; IPython.embed()
if FuzzyZero(A, epsilon):
log.warn(
"(final fix) A is zero. Don't know how to fix this case"
)
return ParabolicCurve()
t0new = mp.fdiv(Sub(Sub(vmnew, v0), B), A)
if (t0new < 0):
log.debug("(final fix) t0new is negative")
return ParabolicCurve()
t1new = Sub(newDuration, t0new)
a0new = Add(A, Mul(mp.fdiv(one, t0new), B))
a1new = Add(A, Mul(mp.fdiv(one, Neg(t1new)), B))
ramp1 = Ramp(v0, a0new, t0new, x0)
ramp2 = Ramp(ramp1.v1, a1new, t1new)
newCurve = ParabolicCurve([ramp1, ramp2])
else:
ramp1 = Ramp(v0, a0new, t0new, x0)
ramp3 = Ramp(ramp1.v1, a1new, t1new)
ramp2 = Ramp(ramp1.v1, zero, Sub(newDuration,
Add(t0new, t1new)))
newCurve = ParabolicCurve([ramp1, ramp2, ramp3])
# import IPython; IPython.embed()
return newCurve
else:
ramp1 = Ramp(v0, a0, t0, x0)
ramp2 = Ramp(ramp1.v1, a1, t1)
newCurve = ParabolicCurve([ramp1, ramp2])
return newCurve
def Interpolate1DFixedDuration(x0, x1, v0, v1, newDuration, vm, am):
x0 = ConvertFloatToMPF(x0)
x1 = ConvertFloatToMPF(x1)
v0 = ConvertFloatToMPF(v0)
v1 = ConvertFloatToMPF(v1)
vm = ConvertFloatToMPF(vm)
am = ConvertFloatToMPF(am)
newDuration = ConvertFloatToMPF(newDuration)
log.debug("\nx0 = {0}; x1 = {1}; v0 = {2}; v1 = {3}; vm = {4}; am = {5}; newDuration = {6}".\
format(mp.nstr(x0, n=_prec), mp.nstr(x1, n=_prec), mp.nstr(v0, n=_prec), mp.nstr(v1, n=_prec),
mp.nstr(vm, n=_prec), mp.nstr(am, n=_prec), mp.nstr(newDuration, n=_prec)))
# Check inputs
assert(vm > zero)
assert(am > zero)
if (newDuration < -epsilon):
log.info("duration = {0} is negative".format(newDuration))
return ParabolicCurve()
if (newDuration <= epsilon):
# Check if this is a stationary trajectory
if (FuzzyEquals(x0, x1, epsilon) and FuzzyEquals(v0, v1, epsilon)):
log.info("stationary trajectory")
ramp0 = Ramp(v0, 0, 0, x0)
newCurve = ParabolicCurve(ramp0)
return newCurve
else:
log.info("newDuration is too short for any movement to be made")
return ParabolicCurve()
# Correct small discrepancies if any
if (v0 > vm):
if FuzzyEquals(v0, vm, epsilon):
v0 = vm
else:
log.info("v0 > vm: {0} > {1}".format(v0, vm))
return ParabolicCurve()
elif (v0 < -vm):
if FuzzyEquals(v0, -vm, epsilon):
v0 = -vm
else:
log.info("v0 < -vm: {0} < {1}".format(v0, -vm))
return ParabolicCurve()
if (v1 > vm):
if FuzzyEquals(v1, vm, epsilon):
v1 = vm
else:
log.info("v1 > vm: {0} > {1}".format(v1, vm))
return ParabolicCurve()
elif (v1 < -vm):
if FuzzyEquals(v1, -vm, epsilon):
v1 = -vm
else:
log.info("v1 < -vm: {0} < {1}".format(v1, -vm))
return ParabolicCurve()
d = Sub(x1, x0)
# First assume no velocity bound -> re-interpolated trajectory will have only two ramps.
# Solve for a0 and a1 (the acceleration of the first and the last ramps).
# a0 = A + B/t0
# a1 = A + B/(t - t0)
# where t is the (new) total duration, t0 is the (new) duration of the first ramp, and
# A = (v1 - v0)/t
# B = (2d/t) - (v0 + v1).
newDurInverse = mp.fdiv(one, newDuration)
A = Mul(Sub(v1, v0), newDurInverse)
B = Sub(Prod([mp.mpf('2'), d, newDurInverse]), Add(v0, v1))
interval0 = iv.mpf([zero, newDuration]) # initial interval for t0
# Now consider the interval(s) computed from a0's constraints
sum1 = Neg(Add(am, A))
sum2 = Sub(am, A)
C = mp.fdiv(B, sum1)
D = mp.fdiv(B, sum2)
log.debug("\nA = {0}; \nB = {1}; \nC = {2}; \nD = {3}; \nsum1 = {4}; \nsum2 = {5};".\
format(mp.nstr(A, n=_prec), mp.nstr(B, n=_prec), mp.nstr(C, n=_prec), mp.nstr(D, n=_prec),
mp.nstr(sum1, n=_prec), mp.nstr(sum2, n=_prec)))
if (sum1 > zero):
# This implied that the duration is too short
log.debug("the given duration ({0}) is too short.".format(newDuration))
return ParabolicCurve()
if (sum2 < zero):
# This implied that the duration is too short
log.debug("the given duration ({0}) is too short.".format(newDuration))
return ParabolicCurve()
if IsEqual(sum1, zero):
raise NotImplementedError # not yet considered
elif sum1 > epsilon:
log.debug("sum1 > 0. This implies that newDuration is too short.")
return ParabolicCurve()
else:
interval1 = iv.mpf([C, inf])
if IsEqual(sum2, zero):
raise NotImplementedError # not yet considered
elif sum2 > epsilon:
interval2 = iv.mpf([D, inf])
else:
log.debug("sum2 < 0. This implies that newDuration is too short.")
return ParabolicCurve()
if Sub(interval2.a, interval1.b) > epsilon or Sub(interval1.a, interval2.b) > epsilon:
# interval1 and interval2 do not intersect each other
return ParabolicCurve()
# interval3 = interval1 \cap interval2 : valid interval for t0 computed from a0's constraints
interval3 = iv.mpf([max(interval1.a, interval2.a), min(interval1.b, interval2.b)])
# Now consider the interval(s) computed from a1's constraints
if IsEqual(sum1, zero):
raise NotImplementedError # not yet considered
elif sum1 > epsilon:
log.debug("sum1 > 0. This implies that newDuration is too short.")
return ParabolicCurve()
else:
interval4 = iv.mpf([Neg(inf), Add(C, newDuration)])
if IsEqual(sum2, zero):
raise NotImplementedError # not yet considered
elif sum2 > epsilon:
interval5 = iv.mpf([Neg(inf), Add(D, newDuration)])
else:
log.debug("sum2 < 0. This implies that newDuration is too short.")
return ParabolicCurve()
if Sub(interval5.a, interval4.b) > epsilon or Sub(interval4.a, interval5.b) > epsilon:
log.debug("interval4 and interval5 do not intersect each other")
return ParabolicCurve()
# interval6 = interval4 \cap interval5 : valid interval for t0 computed from a1's constraints
interval6 = iv.mpf([max(interval4.a, interval5.a), min(interval4.b, interval5.b)])
# import IPython; IPython.embed()
if Sub(interval3.a, interval6.b) > epsilon or Sub(interval6.a, interval3.b) > epsilon:
log.debug("interval3 and interval6 do not intersect each other")
return ParabolicCurve()
# interval7 = interval3 \cap interval6
interval7 = iv.mpf([max(interval3.a, interval6.a), min(interval3.b, interval6.b)])
if Sub(interval0.a, interval7.b) > epsilon or Sub(interval7.a, interval0.b) > epsilon:
log.debug("interval0 and interval7 do not intersect each other")
return ParabolicCurve()
# interval8 = interval0 \cap interval7 : valid interval of t0 when considering all constraints (from a0 and a1)
interval8 = iv.mpf([max(interval0.a, interval7.a), min(interval0.b, interval7.b)])
# import IPython; IPython.embed()
# We choose the value t0 (the duration of the first ramp) by selecting the mid point of the
# valid interval of t0.
t0 = _SolveForT0(A, B, newDuration, interval8)
if t0 is None:
# The fancy procedure fails. Now consider no optimization whatsoever.
# TODO: Figure out why solving fails.
t0 = mp.convert(interval8.mid) # select the midpoint
# return ParabolicCurve()
t1 = Sub(newDuration, t0)
a0 = Add(A, Mul(mp.fdiv(one, t0), B))
if (Abs(t1) < epsilon):
a1 = zero
else:
a1 = Add(A, Mul(mp.fdiv(one, Neg(t1)), B))
assert(Sub(Abs(a0), am) < epsilon) # check if a0 is really below the bound
assert(Sub(Abs(a1), am) < epsilon) # check if a1 is really below the bound
# import IPython; IPython.embed()
# Check if the velocity bound is violated
vp = Add(v0, Mul(a0, t0))
if Abs(vp) > vm:
vmnew = Mul(mp.sign(vp), vm)
D2 = Prod([pointfive, Sqr(Sub(vp, vmnew)), Sub(mp.fdiv(one, a0), mp.fdiv(one, a1))])
# print("D2", end=' ')
# mp.nprint(D2, n=_prec)
# print("vmnew", end=' ')
# mp.nprint(vmnew, n=_prec)
A2 = Sqr(Sub(vmnew, v0))
B2 = Neg(Sqr(Sub(vmnew, v1)))
t0trimmed = mp.fdiv(Sub(vmnew, v0), a0)
t1trimmed = mp.fdiv(Sub(v1, vmnew), a1)
C2 = Sum([Mul(t0trimmed, Sub(vmnew, v0)), Mul(t1trimmed, Sub(vmnew, v1)), Mul(mp.mpf('-2'), D2)])
log.debug("\nA2 = {0}; \nB2 = {1}; \nC2 = {2}; \nD2 = {3};".format(mp.nstr(A2, n=_prec), mp.nstr(B2, n=_prec), mp.nstr(C2, n=_prec), mp.nstr(D2, n=_prec)))
temp = Prod([A2, B2, B2])
initguess = mp.sign(temp)*(Abs(temp)**(1./3.))
root = mp.findroot(lambda x: Sub(Prod([x, x, x]), temp), x0=initguess)
# import IPython; IPython.embed()
log.debug("root = {0}".format(mp.nstr(root, n=_prec)))
a0new = mp.fdiv(Add(A2, root), C2)
if (Abs(a0new) > Add(am, epsilon)):
if FuzzyZero(Sub(Mul(C2, a0new), A2), epsilon):
# The computed a0new is exceeding the bound and its corresponding a1new is
# zero. Therefore, there is no other way to fix this. This is probably because the
# given newDuration is less than the minimum duration (x0, x1, v0, v1, vm, am) can
# get.
log.debug("abs(a0new) > am and a1new = 0; Cannot fix this case. This happens probably because the given newDuration is too short.")
return ParabolicCurve()
a0new = Mul(mp.sign(a0new), am)
if (Abs(a0new) < epsilon):
a1new = mp.fdiv(B2, C2)
if (Abs(a1new) > Add(am, epsilon)):
# Similar to the case above
log.debug("a0new = 0 and abs(a1new) > am; Cannot fix this case. This happens probably because the given newDuration is too short.")
return ParabolicCurve()
else:
if FuzzyZero(Sub(Mul(C2, a0new), A2), epsilon):
# import IPython; IPython.embed()
a1new = 0
else:
a1new = Mul(mp.fdiv(B2, C2), Add(one, mp.fdiv(A2, Sub(Mul(C2, a0new), A2))))
if (Abs(a1new) > Add(am, epsilon)):
a1new = Mul(mp.sign(a1new), am)
a0new = Mul(mp.fdiv(A2, C2), Add(one, mp.fdiv(B2, Sub(Mul(C2, a1new), B2))))
if (Abs(a0new) > Add(am, epsilon)) or (Abs(a1new) > Add(am, epsilon)):
log.warn("Cannot fix acceleration bounds violation")
return ParabolicCurve()
log.debug("\na0 = {0}; \na0new = {1}; \na1 = {2}; \na1new = {3};".format(mp.nstr(a0, n=_prec), mp.nstr(a0new, n=_prec), mp.nstr(a1, n=_prec), mp.nstr(a1new, n=_prec)))
if (Abs(a0new) < epsilon) and (Abs(a1new) < epsilon):
log.warn("Both accelerations are zero. Should we allow this case?")
return ParabolicCurve()
if (Abs(a0new) < epsilon):
# This is likely because v0 is at the velocity bound
t1new = mp.fdiv(Sub(v1, vmnew), a1new)
assert(t1new > 0)
ramp2 = Ramp(v0, a1new, t1new)
t0new = Sub(newDuration, t1new)
assert(t0new > 0)
ramp1 = Ramp(v0, zero, t0new, x0)
newCurve = ParabolicCurve([ramp1, ramp2])
return newCurve
elif (Abs(a1new) < epsilon):
t0new = mp.fdiv(Sub(vmnew, v0), a0new)
assert(t0new > 0)
ramp1 = Ramp(v0, a0new, t0new, x0)
t1new = Sub(newDuration, t0new)
assert(t1new > 0)
ramp2 = Ramp(ramp1.v1, zero, t1new)
newCurve = ParabolicCurve([ramp1, ramp2])
return newCurve
else:
# No problem with those new accelerations
# import IPython; IPython.embed()
t0new = mp.fdiv(Sub(vmnew, v0), a0new)
if (t0new < 0):
log.debug("t0new < 0. The given newDuration not achievable with the given bounds")
return ParabolicCurve()
t1new = mp.fdiv(Sub(v1, vmnew), a1new)
if (t1new < 0):
log.debug("t1new < 0. The given newDuration not achievable with the given bounds")
return ParabolicCurve()
if (Add(t0new, t1new) > newDuration):
# Final fix. Since we give more weight to acceleration bounds, we make the velocity
# bound saturated. Therefore, we set vp to vmnew.
# import IPython; IPython.embed()
if FuzzyZero(A, epsilon):
log.warn("(final fix) A is zero. Don't know how to fix this case")
return ParabolicCurve()
t0new = mp.fdiv(Sub(Sub(vmnew, v0), B), A)
if (t0new < 0):
log.debug("(final fix) t0new is negative")
return ParabolicCurve()
t1new = Sub(newDuration, t0new)
a0new = Add(A, Mul(mp.fdiv(one, t0new), B))
a1new = Add(A, Mul(mp.fdiv(one, Neg(t1new)), B))
ramp1 = Ramp(v0, a0new, t0new, x0)
ramp2 = Ramp(ramp1.v1, a1new, t1new)
newCurve = ParabolicCurve([ramp1, ramp2])
else:
ramp1 = Ramp(v0, a0new, t0new, x0)
ramp3 = Ramp(ramp1.v1, a1new, t1new)
ramp2 = Ramp(ramp1.v1, zero, Sub(newDuration, Add(t0new , t1new)))
newCurve = ParabolicCurve([ramp1, ramp2, ramp3])
# import IPython; IPython.embed()
return newCurve
else:
ramp1 = Ramp(v0, a0, t0, x0)
ramp2 = Ramp(ramp1.v1, a1, t1)
newCurve = ParabolicCurve([ramp1, ramp2])
return newCurve
def gauss(alpha, beta, eps, nloop=30):
n = len(alpha)
if (len(beta) != n):
raise
# initialize
root = alpha[:]
weight = [one]+[zero for i in range(n-1)]
aux = list(map(mp.sqrt, beta[1:]))
aux.append(zero)
for l in range(n):
for j in range(nloop):
for m in range(l, n):
if (m == n-1):
break
if (abs(aux[m]) <= eps*(abs(root[m])+abs(root[m+1]))):
break
dp = root[l]
if (m == l):
break
dg = (root[l+1]-dp)/(two*aux[l])
dr = mp.sqrt(dg*dg+one)
if (mp.sign(dg) < 0):
dr *= -1
dg = root[m]-dp+aux[l]/(dg+dr)
ds = one
dc = one
dp = zero
mml = m-l
for ii in range(1, mml+1):
i = m-ii
df = ds*aux[i]
db = dc*aux[i]
if (abs(df) < abs(dg)):
ds = df/dg
dr = mp.sqrt(ds*ds+one)
aux[i+1] = dg*dr
dc = one/dr
ds = ds*dc
else:
dc = dg/df
dr = mp.sqrt(dc*dc+one)
aux[i+1] = df*dr
ds = one/dr
dc = dc*ds
dg = root[i+1]-dp
dr = (root[i]-dg)*ds+two*dc*db
dp = ds*dr
root[i+1] = dg+dp
dg = dc*dr-db
df = weight[i+1]
weight[i+1] = ds*weight[i]+dc*df
weight[i] = dc*weight[i]-ds*df
root[l] = root[l]-dp
aux[l] = dg
aux[m] = zero
if (j >= nloop-1):
raise
# scale, sort and return
return (list(t) for t in zip(*sorted(zip(root, [beta[0]*w*w for w in weight]))))
def _Stretch1D(curve, newDuration, vm, am):
log.debug("\nx0 = {0}; x1 = {1}; v0 = {2}; v1 = {3}; vm = {4}; am = {5}; prevDuration = {6}; newDuration = {7}".\
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(vm, n=_prec), mp.nstr(am, n=_prec), mp.nstr(curve.duration, n=_prec),
mp.nstr(newDuration, n=_prec)))
# Check types
if type(newDuration) is not mp.mpf:
newDuration = mp.mpf("{:.15e}".format(newDuration))
if type(vm) is not mp.mpf:
vm = mp.mpf("{:.15e}".format(vm))
if type(am) is not mp.mpf:
am = mp.mpf("{:.15e}".format(am))
# Check inputs
# assert(newDuration > curve.duration)
assert(vm > zero)
assert(am > zero)
if (newDuration < -epsilon):
return ParabolicCurve()
if (newDuration <= epsilon):
# Check if this is a stationary trajectory
if (FuzzyEquals(curve.x0, curve.EvalPos(x1), epsilon) and FuzzyEquals(curve.v0, curve.v1, epsilon)):
ramp0 = Ramp(curve.v0, 0, 0, curve.x0)
newCurve = ParabolicCurve(ramp0)
return newCurve
else:
# newDuration is too short to any movement to be made
return ParabolicCurve()
v0 = curve[0].v0
v1 = curve[-1].v1
d = curve.d
# First assume no velocity bound -> re-interpolated trajectory will have only two ramps.
# Solve for a0 and a1 (the acceleration of the first and the last ramps).
# a0 = A + B/t0
# a1 = A + B/(t - t0)
# where t is the (new) total duration, t0 is the (new) duration of the first ramp, and
# A = (v1 - v0)/t
# B = (2d/t) - (v0 + v1).
newDurInverse = mp.fdiv(one, newDuration)
A = Mul(Sub(v1, v0), newDurInverse)
B = Sub(Prod([mp.mpf('2'), d, newDurInverse]), Add(v0, v1))
interval0 = iv.mpf([zero, newDuration]) # initial interval for t0
# Now consider the interval(s) computed from a0's constraints
sum1 = Neg(Add(am, A))
sum2 = Sub(am, A)
C = mp.fdiv(B, sum1)
D = mp.fdiv(B, sum2)
log.debug("\nA = {0}; \nB = {1}; \nC = {2}; \nD = {3}; \nsum1 = {4}; \nsum2 = {5};".\
format(mp.nstr(A, n=_prec), mp.nstr(B, n=_prec), mp.nstr(C, n=_prec), mp.nstr(D, n=_prec),
mp.nstr(sum1, n=_prec), mp.nstr(sum2, n=_prec)))
assert(not (sum1 > zero))
assert(not (sum2 < zero))
if IsEqual(sum1, zero):
raise NotImplementedError # not yet considered
elif sum1 > epsilon:
log.debug("sum1 > 0. This implies that newDuration is too short.")
return ParabolicCurve()
else:
interval1 = iv.mpf([C, inf])
if IsEqual(sum2, zero):
raise NotImplementedError # not yet considered
elif sum2 > epsilon:
interval2 = iv.mpf([D, inf])
else:
log.debug("sum2 < 0. This implies that newDuration is too short.")
return ParabolicCurve()
if Sub(interval2.a, interval1.b) > epsilon or Sub(interval1.a, interval2.b) > epsilon:
# interval1 and interval2 do not intersect each other
return ParabolicCurve()
# interval3 = interval1 \cap interval2 : valid interval for t0 computed from a0's constraints
interval3 = iv.mpf([max(interval1.a, interval2.a), min(interval1.b, interval2.b)])
# Now consider the interval(s) computed from a1's constraints
if IsEqual(sum1, zero):
raise NotImplementedError # not yet considered
elif sum1 > epsilon:
log.debug("sum1 > 0. This implies that newDuration is too short.")
return ParabolicCurve()
else:
interval4 = iv.mpf([Neg(inf), Add(C, newDuration)])
if IsEqual(sum2, zero):
raise NotImplementedError # not yet considered
elif sum2 > epsilon:
interval5 = iv.mpf([Neg(inf), Add(D, newDuration)])
else:
log.debug("sum2 < 0. This implies that newDuration is too short.")
return ParabolicCurve()
if Sub(interval5.a, interval4.b) > epsilon or Sub(interval4.a, interval5.b) > epsilon:
# interval4 and interval5 do not intersect each other
return ParabolicCurve()
# interval6 = interval4 \cap interval5 : valid interval for t0 computed from a1's constraints
interval6 = iv.mpf([max(interval4.a, interval5.a), min(interval4.b, interval5.b)])
# import IPython; IPython.embed()
if Sub(interval3.a, interval6.b) > epsilon or Sub(interval6.a, interval3.b) > epsilon:
# interval3 and interval6 do not intersect each other
return ParabolicCurve()
# interval7 = interval3 \cap interval6
interval7 = iv.mpf([max(interval3.a, interval6.a), min(interval3.b, interval6.b)])
if Sub(interval0.a, interval7.b) > epsilon or Sub(interval7.a, interval0.b) > epsilon:
# interval0 and interval7 do not intersect each other
return ParabolicCurve()
# interval8 = interval0 \cap interval7 : valid interval of t0 when considering all constraints (from a0 and a1)
interval8 = iv.mpf([max(interval0.a, interval7.a), min(interval0.b, interval7.b)])
# import IPython; IPython.embed()
# We choose the value t0 (the duration of the first ramp) by selecting the mid point of the
# valid interval of t0.
t0 = _SolveForT0(A, B, newDuration, interval8)
if t0 is None:
# The fancy procedure fails. Now consider no optimization whatsoever.
# TODO: Figure out why solving fails.
t0 = mp.convert(interval8.mid) # select the midpoint
# return ParabolicCurve()
t1 = Sub(newDuration, t0)
a0 = Add(A, Mul(mp.fdiv(one, t0), B))
if (Abs(t1) < epsilon):
a1 = zero
else:
a1 = Add(A, Mul(mp.fdiv(one, Neg(t1)), B))
assert(Sub(Abs(a0), am) < epsilon) # check if a0 is really below the bound
assert(Sub(Abs(a1), am) < epsilon) # check if a1 is really below the bound
# import IPython; IPython.embed()
# Check if the velocity bound is violated
vp = Add(v0, Mul(a0, t0))
if Abs(vp) > vm:
vmnew = Mul(mp.sign(vp), vm)
D2 = Prod([pointfive, Sqr(Sub(vp, vmnew)), Sub(mp.fdiv(one, a0), mp.fdiv(one, a1))])
# print "D2",
# mp.nprint(D2, n=_prec)
# print "vmnew",
# mp.nprint(vmnew, n=_prec)
A2 = Sqr(Sub(vmnew, v0))
B2 = Neg(Sqr(Sub(vmnew, v1)))
t0trimmed = mp.fdiv(Sub(vmnew, v0), a0)
t1trimmed = mp.fdiv(Sub(v1, vmnew), a1)
C2 = Sum([Mul(t0trimmed, Sub(vmnew, v0)), Mul(t1trimmed, Sub(vmnew, v1)), Mul(mp.mpf('-2'), D2)])
log.debug("\nA2 = {0}; \nB2 = {1}; \nC2 = {2}; \nD2 = {3};".format(mp.nstr(A2, n=_prec), mp.nstr(B2, n=_prec), mp.nstr(C2, n=_prec), mp.nstr(D2, n=_prec)))
temp = Prod([A2, B2, B2])
initguess = mp.sign(temp)*(Abs(temp)**(1./3.))
root = mp.findroot(lambda x: Sub(Prod([x, x, x]), temp), x0=initguess)
# import IPython; IPython.embed()
log.debug("root = {0}".format(mp.nstr(root, n=_prec)))
a0new = mp.fdiv(Add(A2, root), C2)
if (Abs(a0new) > Add(am, epsilon)):
a0new = Mul(mp.sign(a0new), am)
if (Abs(a0new) < epsilon):
a1new = mp.fdiv(B2, C2)
if (Abs(a1new) > Add(am, epsilon)):
log.warn("abs(a1new) > am; cannot fix this case")
# Cannot fix this case
return ParabolicCurve()
else:
if FuzzyZero(Sub(Mul(C2, a0new), A2), epsilon):
# import IPython; IPython.embed()
a1new = 0
else:
a1new = Mul(mp.fdiv(B2, C2), Add(one, mp.fdiv(A2, Sub(Mul(C2, a0new), A2))))
if (Abs(a1new) > Add(am, epsilon)):
a1new = Mul(mp.sign(a1new), am)
a0new = Mul(mp.fdiv(A2, C2), Add(one, mp.fdiv(B2, Sub(Mul(C2, a1new), B2))))
if (Abs(a0new) > Add(am, epsilon)) or (Abs(a1new) > Add(am, epsilon)):
log.warn("Cannot fix acceleration bounds violation")
return ParabolicCurve()
log.debug("\na0 = {0}; \na0new = {1}; \na1 = {2}; \na1new = {3};".format(mp.nstr(a0, n=_prec), mp.nstr(a0new, n=_prec), mp.nstr(a1, n=_prec), mp.nstr(a1new, n=_prec)))
if (Abs(a0new) < epsilon) and (Abs(a1new) < epsilon):
log.warn("Both accelerations are zero. Should we allow this case?")
return ParabolicCurve()
if (Abs(a0new) < epsilon):
# This is likely because v0 is at the velocity bound
t1new = mp.fdiv(Sub(v1, vmnew), a1new)
assert(t1new > 0)
ramp2 = Ramp(v0, a1new, t1new)
t0new = Sub(newDuration, t1new)
assert(t0new > 0)
ramp1 = Ramp(v0, zero, t0new, curve.x0)
newCurve = ParabolicCurve([ramp1, ramp2])
return newCurve
elif (Abs(a1new) < epsilon):
t0new = mp.fdiv(Sub(vmnew, v0), a0new)
assert(t0new > 0)
ramp1 = Ramp(v0, a0new, t0new, curve.x0)
t1new = Sub(newDuration, t0new)
assert(t1new > 0)
ramp2 = Ramp(ramp1.v1, zero, t1new)
newCurve = ParabolicCurve([ramp1, ramp2])
return newCurve
else:
# No problem with those new accelerations
# import IPython; IPython.embed()
t0new = mp.fdiv(Sub(vmnew, v0), a0new)
assert(t0new > 0)
t1new = mp.fdiv(Sub(v1, vmnew), a1new)
assert(t1new > 0)
if (Add(t0new, t1new) > newDuration):
# Final fix. Since we give more weight to acceleration bounds, we make the velocity
# bound saturated. Therefore, we set vp to vmnew.
# import IPython; IPython.embed()
t0new = mp.fdiv(Sub(Sub(vmnew, v0), B), A)
t1new = Sub(newDuration, t0new)
assert(t1new > zero)
a0new = Add(A, Mul(mp.fdiv(one, t0new), B))
a1new = Add(A, Mul(mp.fdiv(one, Neg(t1new)), B))
ramp1 = Ramp(v0, a0new, t0new, curve.x0)
ramp2 = Ramp(ramp1.v1, a1new, t1new)
newCurve = ParabolicCurve([ramp1, ramp2])
else:
# t0new = mp.fdiv(Sub(vmnew, v0), a0new)
# assert(t0new > 0)
ramp1 = Ramp(v0, a0new, t0new, curve.x0)
# t1new = mp.fdiv(Sub(v1, vmnew), a1new)
# assert(t1new > 0)
ramp3 = Ramp(ramp1.v1, a1new, t1new)
# print "a1",
# mp.nprint(a1, n=_prec)
# print "a1new",
# mp.nprint(a1new, n=_prec)
# print "t0trimmed",
# mp.nprint(t0trimmed, n=_prec)
# print "t0new",
# mp.nprint(t0new, n=_prec)
# print "t1trimmed",
# mp.nprint(t1trimmed, n=_prec)
# print "t1new",
# mp.nprint(t1new, n=_prec)
# print "T",
# mp.nprint(newDuration, n=_prec)
ramp2 = Ramp(ramp1.v1, zero, Sub(newDuration, Add(t0new , t1new)))
newCurve = ParabolicCurve([ramp1, ramp2, ramp3])
# import IPython; IPython.embed()
return newCurve
else:
ramp1 = Ramp(v0, a0, t0, curve.x0)
ramp2 = Ramp(ramp1.v1, a1, t1)
newCurve = ParabolicCurve([ramp1, ramp2])
return newCurve
