def main(
ramp_start=START,
ramp_end=START + RISE,
ramp_seconds=RISE * 60,
speed=71,
feel_min=20,
feel_max=90,
active_seconds=10,
inactive_seconds_min=5,
inactive_seconds_max=15,
):
ramp = Ramp(ramp_start, ramp_end, ramp_seconds)
with commander() as cmd:
cmd.set_power('H')
cmd.set_mode(Mode.TRAINING)
cmd.set_speed(speed)
while True:
# 35 + (-5..5) = 30..40
feel = randint(feel_min, feel_max)
cmd.set_feel(feel)
level = ramp.get_value() + fate_dice(5)
cmd.set_level('A', level)
logger.info('Sleeping in active cycle for %d seconds',
active_seconds)
sleep(active_seconds)
cmd.set_level('A', 0)
inactive_seconds = randint(inactive_seconds_min,
inactive_seconds_max)
logger.info('Sleeping in inactive cycle for %d seconds',
inactive_seconds)
sleep(inactive_seconds)
def main(
ramp_start=START,
ramp_end=START + RISE,
ramp_seconds=RISE * 60,
feel=90,
osc_multiplier=3,
active_seceonds=2,
inactive_seconds_min=2,
inactive_seconds_max=10,
):
ramp = Ramp(ramp_start, ramp_end, ramp_seconds)
seq = Sequence()
time_osc = Oscillator(inactive_seconds_min, inactive_seconds_max, 120)
with commander() as cmd:
cmd.set_power('H')
cmd.set_mode(Mode.CONTINUOUS)
cmd.set_feel(feel)
while True:
base_level = ramp.get_value() + osc_multiplier * seq.get_value()
cmd.set_level('A', base_level)
sleep(active_seceonds)
cmd.set_level('A', 0)
# inactive_seconds = randint(inactive_seconds_min, inactive_seconds_max)
inactive_seconds = time_osc.get_value()
sleep(inactive_seconds)
def main(
ramp_start=START,
ramp_end=START + RISE,
ramp_seconds=RISE * 60,
feel=90,
speed=30,
seq_min=-20,
seq_max=20,
seq_step=2,
step_seconds=5,
):
ramp = Ramp(ramp_start, ramp_end, ramp_seconds)
seq = Sequence(min_value=seq_min, max_value=seq_max, step=seq_step)
with commander() as cmd:
cmd.set_mode(Mode.PULSE)
cmd.set_power('H')
cmd.set_speed(speed)
cmd.set_feel(feel)
while True:
level = ramp.get_value()
level += max(seq.get_value(), 0)
cmd.set_level('A', level)
sleep(step_seconds)
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 _extract(self, zipped_lists):
self.ramps = []
for max_delta, points in zipped_lists:
ramp = Ramp(self.bipolar, self.max_time_slices)
if ramp.construct(max_delta, points) != None:
self.ramps.append(ramp)
else:
self._invalidate()
break
return self.ramps
def __init__(self) -> None:
self._joystick = Joystick(FORWARD_PIN, REVERSE_PIN, LEFT_PIN,
RIGHT_PIN)
self._left_ramp = Ramp(RAMP_SIZE, RAMP_BREAK_FACTOR)
self._right_ramp = Ramp(RAMP_SIZE, RAMP_BREAK_FACTOR)
self._left_motor_controler = MotorControler(LEFT_MOTOR_PWM,
LEFT_MOTOR_A, LEFT_MOTOR_B)
self._right_motor_controler = MotorControler(RIGHT_MOTOR_PWM,
RIGHT_MOTOR_A,
RIGHT_MOTOR_B)
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 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 robotInit(self):
# Instances of classes
# Instantiate Subsystems
self.drivetrain = Drivetrain(self)
self.hp_intake = Hp_Intake()
self.ramp = Ramp()
self.shifters = Shifters()
# instantiate Encoders for drivetrain?
#self.encoders = Encoders(self)
# Instantiate Joysticks
self.left_joy = wpilib.Joystick(0)
self.right_joy = wpilib.Joystick(1)
# Instantiate Xbox
self.xbox = wpilib.Joystick(2)
# Instantiate OI; must be AFTER joysticks are inited
self.oi = OI(self)
self.timer = wpilib.Timer()
self.loops = 0
# untested vision
#XXX might crash sim
wpilib.CameraServer.launch("vision.py:main")
def xofangle(x):
myRamp = Ramp(100, 0)
myProjectile = Projectile(math.sqrt(2), x, 0, myRamp)
myProjectile.calcxtFall(0.00000000000000001)
return myProjectile.xFall
def main(
ramp_start=START,
ramp_end=START + RISE,
ramp_seconds=RISE * 60,
warn_adjustment=2,
feel=80,
warn_seconds=3,
bang_adjustment_min=10,
bang_adjustment_max=20,
active_seconds=1,
inactive_seconds_min=10,
inactive_seconds_max=30,
):
ramp = Ramp(ramp_start, ramp_end, ramp_seconds)
seq = Sequence()
time_osc = Oscillator(inactive_seconds_min, inactive_seconds_max, 120)
bang_adjustment = bang_adjustment_min
with commander() as cmd:
cmd.set_mode(Mode.CONTINUOUS)
cmd.set_power('H')
cmd.set_feel(feel)
while True:
active_level = ramp.get_value()
warn_level = active_level // warn_adjustment
if randint(1, 6) == 1:
# bang
active_level += bang_adjustment
bang_adjustment = min(bang_adjustment + 1, bang_adjustment_max)
cmd.set_level('A', warn_level)
sleep(warn_seconds)
cmd.set_level('A', active_level)
sleep(active_seconds)
cmd.set_level('A', 0)
# inactive_seconds = randint(inactive_seconds_min, inactive_seconds_max)
inactive_seconds = time_osc.get_value()
sleep(inactive_seconds)
def main(
ramp_start=40,
ramp_end=60,
ramp_seconds=20*60,
short_ramp_rounds=20,
short_ramp_multiplier=2,
speed=40,
feel=80,
):
ramp = Ramp(ramp_start, ramp_end, ramp_seconds)
with commander() as cmd:
cmd.set_mode(Mode.PULSE)
cmd.set_feel(feel)
cmd.set_speed(speed)
while True:
base_value = ramp.get_value()
for short_ramp_adjustment in range(short_ramp_rounds):
cmd.set_level('A', base_value + short_ramp_adjustment * short_ramp_multiplier)
cmd.set_level('A', 0)
def main(
ramp_start=START,
ramp_end=START + RISE,
ramp_seconds=RISE * 60,
feel=80,
speed=65,
osc_max=10,
osc_period_seconds=120,
step_seconds=5,
):
ramp = Ramp(ramp_start, ramp_end, ramp_seconds)
osc = Oscillator(0, osc_max, osc_period_seconds)
with commander() as cmd:
cmd.set_mode(Mode.WATERFALL)
cmd.set_power('H')
cmd.set_speed(speed)
cmd.set_feel(feel)
while True:
base_level = ramp.get_value() + osc.get_value()
cmd.set_level('A', base_level)
sleep(step_seconds)
def main(
ramp_start=START,
ramp_end=START + RISE,
ramp_seconds=RISE * 60,
warn_adjustment=10,
feel=80,
speed=90,
warn_seconds=3,
osc_multiplier=2,
active_seconds=5,
inactive_seconds_min=5,
inactive_seconds_max=15,
):
ramp = Ramp(ramp_start, ramp_end, ramp_seconds)
seq = Sequence()
time_osc = Oscillator(inactive_seconds_min, inactive_seconds_max, 120)
with commander() as cmd:
cmd.set_power('H')
cmd.set_mode(Mode.PULSE)
cmd.set_feel(feel)
cmd.set_speed(speed)
while True:
base_level = ramp.get_value() + osc_multiplier * seq.get_value()
warn_level = base_level - warn_adjustment
cmd.set_level('A', warn_level)
sleep(warn_seconds)
cmd.set_level('A', base_level)
sleep(active_seconds)
cmd.set_level('A', 0)
# inactive_seconds = randint(inactive_seconds_min, inactive_seconds_max)
inactive_seconds = time_osc.get_value()
sleep(inactive_seconds)
class Powertrain:
def __init__(self) -> None:
self._joystick = Joystick(FORWARD_PIN, REVERSE_PIN, LEFT_PIN,
RIGHT_PIN)
self._left_ramp = Ramp(RAMP_SIZE, RAMP_BREAK_FACTOR)
self._right_ramp = Ramp(RAMP_SIZE, RAMP_BREAK_FACTOR)
self._left_motor_controler = MotorControler(LEFT_MOTOR_PWM,
LEFT_MOTOR_A, LEFT_MOTOR_B)
self._right_motor_controler = MotorControler(RIGHT_MOTOR_PWM,
RIGHT_MOTOR_A,
RIGHT_MOTOR_B)
def loop(self):
start_time = time.ticks_ms()
while True:
try:
# delta = time.ticks_diff(time.ticks_ms(), start_time)
# print("delta", delta)
delta = 10
left_direction, right_direction = self._joystick.get()
print("direction", left_direction, right_direction)
self._left_ramp.update(left_direction, delta)
self._right_ramp.update(right_direction, delta)
left_consign = self._left_ramp.get_normalized(750)
self._left_motor_controler.set(left_consign)
right_consign = self._right_ramp.get_normalized(750)
self._right_motor_controler.set(right_consign)
print("")
time.sleep_ms(10)
except Exception as e:
print(e)
start_time = time.ticks_ms()
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 _ImposeJointLimitFixedDuration(curve, xmin, xmax, vm, am):
bmin, bmax = curve.GetPeaks()
if (bmin >= Sub(xmin, epsilon)) and (bmax <= Add(xmax, epsilon)):
# Joint limits are not violated
return curve
duration = curve.duration
x0 = curve.x0
x1 = curve.EvalPos(duration)
v0 = curve.v0
v1 = curve.v1
bt0 = inf
bt1 = inf
ba0 = inf
ba1 = inf
bx0 = inf
bx1 = inf
if (v0 > zero):
bt0 = _BrakeTime(x0, v0, xmax)
bx0 = xmax
ba0 = _BrakeAccel(x0, v0, xmax)
elif (v0 < zero):
bt0 = _BrakeTime(x0, v0, xmin)
bx0 = xmin
ba0 = _BrakeAccel(x0, v0, xmin)
if (v1 < zero):
bt1 = _BrakeTime(x1, -v1, xmax)
bx1 = xmax
ba1 = _BrakeAccel(x1, -v1, xmax)
elif (v1 > zero):
bt1 = _BrakeTime(x1, -v1, xmin)
bx1 = xmin
ba1 = _BrakeAccel(x1, -v1, xmin)
# import IPython; IPython.embed()
newCurve = ParabolicCurve()
if ((bt0 < duration) and (Abs(ba0) < Add(am, epsilon))):
# Case IIa
log.debug("Case IIa")
firstRamp = Ramp(v0, ba0, bt0, x0)
if (Abs(Sub(x1, bx0)) < Mul(Sub(duration, bt0), vm)):
tempCurve1 = Interpolate1D(bx0, x1, zero, v1, vm, am)
if not tempCurve1.isEmpty:
if (Sub(duration, bt0) >= tempCurve1.duration):
tempCurve2 = _Stretch1D(tempCurve1, Sub(duration, bt0), vm,
am)
if not tempCurve2.isEmpty:
tempbmin, tempbmax = tempCurve2.GetPeaks()
if not ((tempbmin < Sub(xmin, epsilon)) or
(tempbmax > Add(xmax, epsilon))):
log.debug("Case IIa successful")
newCurve = ParabolicCurve([firstRamp] +
tempCurve2.ramps)
if ((bt1 < duration) and (Abs(ba1) < Add(am, epsilon))):
# Case IIb
log.debug("Case IIb")
lastRamp = Ramp(0, ba1, bt1, bx1)
if (Abs(Sub(x0, bx1)) < Mul(Sub(duration, bt1), vm)):
tempCurve1 = Interpolate1D(x0, bx1, v0, zero, vm, am)
if not tempCurve1.isEmpty:
if (Sub(duration, bt1) >= tempCurve1.duration):
tempCurve2 = _Stretch1D(tempCurve1, Sub(duration, bt1), vm,
am)
if not tempCurve2.isEmpty:
tempbmin, tempbmax = tempCurve2.GetPeaks()
if not ((tempbmin < Sub(xmin, epsilon)) or
(tempbmax > Add(xmax, epsilon))):
log.debug("Case IIb successful")
newCurve = ParabolicCurve(tempCurve2.ramps +
[lastRamp])
if (bx0 == bx1):
# Case III
if (Add(bt0, bt1) < duration) and (max(Abs(ba0), Abs(ba1)) < Add(
am, epsilon)):
log.debug("Case III")
ramp0 = Ramp(v0, ba0, bt0, x0)
ramp1 = Ramp(zero, zero, Sub(duration, Add(bt0, bt1)))
ramp2 = Ramp(zero, ba1, bt1)
newCurve = ParabolicCurve([ramp0, ramp1, ramp2])
else:
# Case IV
if (Add(bt0, bt1) < duration) and (max(Abs(ba0), Abs(ba1)) < Add(
am, epsilon)):
log.debug("Case IV")
firstRamp = Ramp(v0, ba0, bt0, x0)
lastRamp = Ramp(zero, ba1, bt1)
if (Abs(Sub(bx0, bx1)) < Mul(Sub(duration, Add(bt0, bt1)), vm)):
tempCurve1 = Interpolate1D(bx0, bx1, zero, zero, vm, am)
if not tempCurve1.isEmpty:
if (Sub(duration, Add(bt0, bt1)) >= tempCurve1.duration):
tempCurve2 = _Stretch1D(tempCurve1,
Sub(duration, Add(bt0, bt1)),
vm, am)
if not tempCurve2.isEmpty:
tempbmin, tempbmax = tempCurve2.GetPeaks()
if not ((tempbmin < Sub(xmin, epsilon)) or
(tempbmax > Add(xmax, epsilon))):
log.debug("Case IV successful")
newCurve = ParabolicCurve([firstRamp] +
tempCurve2.ramps +
[lastRamp])
if (newCurve.isEmpty):
log.warn("Cannot solve for a bounded trajectory")
log.warn("x0 = {0}; x1 = {1}; v0 = {2}; v1 = {3}; xmin = {4}; xmax = {5}; vm = {6}; am = {7}; duration = {8}".\
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(xmin, n=_prec), mp.nstr(xmax, n=_prec),
mp.nstr(vm, n=_prec), mp.nstr(am, n=_prec), mp.nstr(duration, n=_prec)))
return newCurve
newbmin, newbmax = newCurve.GetPeaks()
if (newbmin < Sub(xmin, epsilon)) or (newbmax > Add(xmax, epsilon)):
log.warn(
"Solving finished but the trajectory still violates the bounds")
# import IPython; IPython.embed()
log.warn("x0 = {0}; x1 = {1}; v0 = {2}; v1 = {3}; xmin = {4}; xmax = {5}; vm = {6}; am = {7}; duration = {8}".\
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(xmin, n=_prec), mp.nstr(xmax, n=_prec),
mp.nstr(vm, n=_prec), mp.nstr(am, n=_prec), mp.nstr(duration, n=_prec)))
return ParabolicCurve()
log.debug("Successfully fixed x-bound violation")
return newCurve
class TestRamp(unittest.TestCase):
def setUp(self) -> None:
self.r = Ramp(10)
def test_forward_1(self):
self.r.update(1, 1)
result = self.r.get()
expected = 1
self.assertEqual(result, expected)
def test_forward_2(self):
self.r.update(1, 1)
self.r.update(1, 10)
result = self.r.get()
expected = 10
self.assertEqual(result, expected)
def test_forward_release_1(self):
self.r.update(1, 5)
self.r.update(0, 3)
result = self.r.get()
expected = 2
self.assertEqual(result, expected)
def test_forward_release_2(self):
self.r.update(1, 5)
self.r.update(0, 6)
result = self.r.get()
expected = 0
self.assertEqual(result, expected)
def test_forward_brake_1(self):
self.r.update(1, 7)
self.r.update(-1, 3)
result = self.r.get()
expected = 1
self.assertEqual(result, expected)
def test_forward_brake_2(self):
self.r.update(1, 5)
self.r.update(-1, 6)
result = self.r.get()
expected = -7
self.assertEqual(result, expected)
def test_reverse_1(self):
self.r.update(-1, 1)
result = self.r.get()
expected = -1
self.assertEqual(result, expected)
def test_reverse_2(self):
self.r.update(-11, 1)
self.r.update(-1, 10)
result = self.r.get()
expected = -10
self.assertEqual(result, expected)
def test_reverse_release_1(self):
self.r.update(-1, 5)
self.r.update(0, 3)
result = self.r.get()
expected = -2
self.assertEqual(result, expected)
def test_reverse_release_2(self):
self.r.update(-1, 5)
self.r.update(0, 6)
result = self.r.get()
expected = 0
self.assertEqual(result, expected)
def test_reverse_brake_1(self):
self.r.update(-1, 7)
self.r.update(1, 3)
result = self.r.get()
expected = -1
self.assertEqual(result, expected)
def test_reverse_brake_2(self):
self.r.update(-1, 5)
self.r.update(1, 6)
result = self.r.get()
expected = 7
self.assertEqual(result, expected)
def test_get_normalized(self):
self.r.update(1, 10)
result = self.r.get_normalized()
expected = 1023
self.assertEqual(result, expected)
def setUp(self) -> None:
self.r = Ramp(10)
from ramp import Ramp
from target import Target
from angleMath import AngleMath
from projectile import Projectile
from learn import Learn
import math
from scipy.misc import derivative
import graphics
from myConstants import myConstants
myRamp = Ramp(700, AngleMath.toRad(15))
myProjectile = Projectile(60, AngleMath.toRad(20), 100, myRamp)
graphics.initialGraphics(myProjectile)
print("initial x0:" + str(myProjectile.x0))
print("initial y0:" + str(myProjectile.y0))
myRamp.printStats()
myProjectile.calcxtFall(0.000001)
t = 0.0
# for i in range(1, len(myProjectile.tFallList)):
for i in range(0, len(myProjectile.tList)):
if (myProjectile.tList[i] >= t):
myProjectile.x = myProjectile.xList[i]
myProjectile.y = myProjectile.yList[i]
graphics.move(graphics.player, myProjectile, t)
t += myConstants.myRate