def test_target_position_vmax_VI(self):
"""Running in reverse."""
solver = TrajectorySolver(20, 5, 2.5, -20, -5, -2.5)
# zero endpoints
spline = solver.target_position_vmax(Knot(250, 0, 0, 0), Knot(0, 0, 0, 0))
## self.plot_it(spline, solver, "test_target_position_vmax_VI_i")
self.validate_spline(spline, solver)
self.assertAlmostEqual(spline.q[-1], 0, self.PLACES)
self.assertAlmostEqual(spline.v[-1], 0, self.PLACES)
self.assertAlmostEqual(spline.a[-1], 0, self.PLACES)
self.assertAlmostEqual(spline.t[-1], 18.5, self.PLACES)
# Doesn't hit either max or min accel, with non-zero endopints
solver = TrajectorySolver(5, 10, 2.5, -5, -10, -2.5)
spline = solver.target_position_vmax(Knot(200,2,-2,0), Knot(0,-4,-1,0))
## self.plot_it(spline, solver)
self.validate_spline(spline, solver)
self.assertAlmostEqual(spline.q[-1], 0, self.PLACES)
self.assertAlmostEqual(spline.v[-1], -4, self.PLACES)
self.assertAlmostEqual(spline.a[-1], -1, self.PLACES)
self.assertAlmostEqual(spline.t[-1], 41.807119601154, self.PLACES)
# non-zeo endpoints, asymetrical limits
# FIXME: Exceeding accel limit?
solver = TrajectorySolver(20, 5, 2.5, -20, -10, -2.5)
spline = solver.target_position_vmax(Knot(300, 5, 2, 0), Knot(100, 6, -2, 0))
## self.plot_it(spline, solver)
self.validate_spline(spline, solver)
self.assertAlmostEqual(spline.q[-1], 100, self.PLACES)
self.assertAlmostEqual(spline.v[-1], 6, self.PLACES)
self.assertAlmostEqual(spline.a[-1], -2, self.PLACES)
self.assertAlmostEqual(spline.t[-1], 21.15796029495, self.PLACES)
def test_target_position_vmax_II(self):
"""May not hit max or min accelerations"""
# Doesn't hit either max or min accel. zero endpoints
solver = TrajectorySolver(20, 10, 2.5)
spline = solver.target_position_vmax(Knot(0,0,0,0), Knot(200,0,0,0))
# self.plot_it(spline, solver)
self.validate_spline(spline, solver)
self.assertAlmostEqual(spline.q[-1], 200, self.PLACES)
self.assertAlmostEqual(spline.v[-1], 0, self.PLACES)
self.assertAlmostEqual(spline.a[-1], 0, self.PLACES)
self.assertAlmostEqual(spline.t[-1], 15.656854249, self.PLACES)
# Doesn't hit either max or min accel, with non-zero endopints
spline = solver.target_position_vmax(Knot(0,1,-1,0), Knot(200,2,-2,0))
# self.plot_it(spline, solver)
self.validate_spline(spline, solver)
self.assertAlmostEqual(spline.q[-1], 200, self.PLACES)
self.assertAlmostEqual(spline.v[-1], 2, self.PLACES)
self.assertAlmostEqual(spline.a[-1], -2, self.PLACES)
self.assertAlmostEqual(spline.t[-1], 14.879489503, self.PLACES)
# hits a_max, a_min, and v_max exactly
solver = TrajectorySolver(10, 5, 2.5)
spline = solver.target_position_vmax(Knot(0,0,0,0), Knot(40,0,0,0))
# self.plot_it(spline, solver)
self.validate_spline(spline, solver)
self.assertAlmostEqual(spline.q[-1], 40, self.PLACES)
self.assertAlmostEqual(spline.v[-1], 0, self.PLACES)
self.assertAlmostEqual(spline.a[-1], 0, self.PLACES)
self.assertAlmostEqual(spline.t[-1], 8, self.PLACES)
def test_target_time_XII(self):
"""Problem from testing. Unachievable."""
solver = TrajectorySolver(15, 5, 2.5, 0, -5, -2.5)
initial = Knot(2.0, 15.0, 0.0, 5.4795365136060168)
final = Knot(70.901034075983588, 15.0, 0, 11.706272118671588) # ave speed of 11.065 m/s
self.assertRaises(FatalTrajectoryError,
solver.target_time, initial, final )
def test_target_time_III(self):
"""Insufficient time to achieve target due to low vehicle v_max"""
solver = TrajectorySolver(10, 5, 2.5, 0, -5, -2.5)
initial = Knot(0,0,0,0)
final = Knot(1000, 0, 0, 54)
self.assertRaises(FatalTrajectoryError,
solver.target_time, initial, final) # needs approx 20 m/s cruise, ignoring jerk limits
def test_target_velocity_VII(self):
solver = TrajectorySolver(15, 5, 2.5, 0, -5, -2.5)
initial = Knot(0, 14.5, 2, 0)
final = Knot(None, 15, 0, None)
## spline = solver.target_velocity(initial, final)
## self.plot_it(spline, solver, "test_target_velocity_VII")
self.assertRaises(FatalTrajectoryError, solver.target_velocity, initial, final)
def test_target_position_VI(self):
"""Yet another real-world problem case."""
solver = TrajectorySolver(32, 1., 2.5, -5.0, -5.0, -2.5)
try:
spline = solver.target_position(Knot(0.0, 10.160758018493652, -5.0, 28.024999999999999),
Knot(67.214868918397073, 0, 0, None))
## self.plot_it(spline, solver, "test_target_position_VI")
## self.validate_spline(spline, solver)
self.assertAlmostEqual(spline.q[-1], 67.214868918397073, 5)
self.assertAlmostEqual(spline.v[-1], 0, 5)
self.assertAlmostEqual(spline.a[-1], 0, 5)
self.assertTrue(spline.t[-1] - spline.t[0] < 60)
except OptimizationError, err:
import random
while True:
try:
new_durations = [random.random() for h in err.durations]
print "initial hs:", new_durations
solver.target_position_none(Knot(0.0, 10.160758018493652, -5.0, 28.024999999999999),
Knot(67.214868918397073, 0, 0, None),
new_durations)
except OptimizationError as err:
print "Soln hs:", solver._soln_spline.h
self.plot_it(solver._soln_spline, solver, "test_remix...")
else:
print "Soln hs:", solver._soln_spline.h
self.plot_it(solver._soln_spline, solver, "test_remix...")
break
def test_target_position_vmax_I(self):
"""Hits max and min accelerations."""
solver = TrajectorySolver(20, 5, 2.5)
### Only position is non-zero at endpoints
spline = solver.target_position_vmax(Knot(0,0,0,0), Knot(200, 0, 0, 0))
## self.plot_it(spline, solver)
self.validate_spline(spline, solver)
self.assertAlmostEqual(spline.q[-1], 200, self.PLACES)
self.assertAlmostEqual(spline.v[-1], 0, self.PLACES)
self.assertAlmostEqual(spline.a[-1], 0, self.PLACES)
self.assertAlmostEqual(spline.t[-1], 16, self.PLACES)
### Position and velocity are non-zero at endpoints
spline = solver.target_position_vmax(Knot(100, 2, 0, 0), Knot(300, -2, 0, 0))
## self.plot_it(spline, solver)
self.validate_spline(spline, solver)
self.assertAlmostEqual(spline.q[-1], 300, self.PLACES)
self.assertAlmostEqual(spline.v[-1], -2, self.PLACES)
self.assertAlmostEqual(spline.a[-1], 0, self.PLACES)
self.assertAlmostEqual(spline.t[-1], 16.04, self.PLACES)
### Position, velocity and accel are non-zero at endpoints
spline = solver.target_position_vmax(Knot(100, 2, -2, 0), Knot(300, -2, 2, 0))
## self.plot_it(spline, solver)
self.validate_spline(spline, solver)
self.assertAlmostEqual(spline.q[-1], 300, self.PLACES)
self.assertAlmostEqual(spline.v[-1], -2, self.PLACES)
self.assertAlmostEqual(spline.a[-1], 2, self.PLACES)
self.assertAlmostEqual(spline.t[-1], 18.08906666, self.PLACES)
def test__no_acceleration_predict_II(self):
"""Starts with 0 vel, 0 accel. No accel prediction is mostly meaningless."""
solver = TrajectorySolver(40, 5, 2.5, 0, -5, -2.5)
initial = Knot(10,0,0,0)
final = Knot(50,5,-1,None)
spline = solver._no_acceleration_predict(initial, final)
self.assertEqual(spline.t[-1], float('inf'))
def test_target_velocity_no_delta_v_I(self):
"""Simple case. No acceleration involved."""
solver = TrajectorySolver(40, 5, 2.5, 0, -20, -1.25)
initial = Knot(0, 20, 0, 0)
final = Knot(None, 20, 0, None)
spline = solver.target_velocity(initial, final)
self.validate_spline(spline, solver)
self.validate_endpoints(spline, initial, final, pos=False, time=False)
def test_target_velocity_V(self):
solver = TrajectorySolver(15, 10, 5.0, 0, -10, -5.0)
initial = Knot(0, 11.51496564563041, 5.6694744752383066, 0)
final = Knot(None, 14.732396990426981, 0, None)
spline = solver.target_velocity(initial, final)
## self.plot_it(spline, solver, "test_target_velocity_V")
self.validate_spline(spline, solver)
self.validate_endpoints(spline, initial, final, pos=False, time=False)
def test_target_position_XI(self):
solver = TrajectorySolver(60, 10, 5, 0, -10, -5)
initial = Knot(1.9999999896260192, 8.5068000742251115, -0.022863034988063191, 447.29479530675786)
final = Knot(42.498999999999995, 2.3999999999999999, 0, None)
spline = solver.target_position(initial, final, initial.vel)
## self.plot_it(spline, solver, "test_target_position_XI")
self.validate_spline(spline, solver)
self.validate_endpoints(spline, initial, final, time=False)
def test_target_time_XXII(self):
solver = TrajectorySolver(67.0, 9.0, 7.0, 0, -9.0, -7.0)
initial = Knot(41.653999342612224, 44.0, 0.0, 5425.3539044854588)
final = Knot(196.25800000000001, 2.3999999999999999, 0, 5432.6250864545336)
spline = solver.target_time(initial, final)
## self.plot_it(spline, solver, "test_target_time_XXI")
self.validate_spline(spline, solver)
self.validate_endpoints(spline, initial, final)
def test_target_time_XVIII(self):
solver = TrajectorySolver(15, 10, 5.0, 0, -10, -5.0)
initial = Knot(23.973000000002685, 15.0, 0.0, 1154.2775841472599)
final = Knot(277.55799999999999, 15.0, 0, 1172.0256197255005)
spline = solver.target_time(initial, final)
## self.plot_it(spline, solver, "test_target_time_XVIII")
self.validate_spline(spline, solver)
self.validate_endpoints(spline, initial, final)
def test_target_time_XXI(self):
solver = TrajectorySolver(20, 7.2, 5.0, 0, -7.2, -5.0)
initial = Knot(9.8130000000001303, 18.114342061230673, 4.3424163075059123, 3.3492945162765952)
final = Knot(709.7650000000001, 20.0, 0, 38.72843888888891)
spline = solver.target_time(initial, final)
## self.plot_it(spline, solver, "test_target_time_XXI")
self.validate_spline(spline, solver)
self.validate_endpoints(spline, initial, final)
def test_target_time_XV(self):
"""Problem from testing."""
solver = TrajectorySolver(15, 5, 2.5, 0, -5, -2.5)
initial = Knot(52.500999999999998, 14.917710067720561, 0, 2924.9500320272209)
final = Knot(643.9559999999999, 15.0, 0, 2964.5960666793744)
spline = solver.target_time(initial, final)
self.validate_spline(spline, solver)
self.validate_endpoints(spline, initial, final)
def test_target_position_amax_III(self):
solver = TrajectorySolver(30, 5, 5, 0, -5, -5)
initial = Knot(21.494, 0.0, 0, 0.0)
final = Knot(57.176000000000002, 3.5, 0, None)
spline = solver.target_position(initial, final, 25.0)
self.plot_it(spline, solver, "test_target_position_amax_III")
self.validate_spline(spline, solver)
self.validate_endpoints(spline, initial, final, time=False)
def test_target_time_XIV(self):
"""Problem from testing."""
solver = TrajectorySolver(15, 5, 2.5, 0, -5, -2.5)
initial = Knot(9.9000001907348629, 0.0, 0.0, 1978.7999999999861)
final = Knot(267.03399999999999, 15.0, 0, 2002.9280666602958)
spline = solver.target_time(initial, final)
self.validate_spline(spline, solver)
self.validate_endpoints(spline, initial, final)
def test_target_time_VIII(self):
"""Another problem that occured during testing"""
solver = TrajectorySolver(60, 5, 2.5, 0, -5, -2.5)
initial = Knot(42.629766162611496, 7.8664068437709354, 0, 47.65161563986365)
final = Knot(351.8905858935297, 15.0, 0, 73.876510498199906)
spline = solver.target_time(initial, final)
## self.plot_it(spline, solver, "test_target_time_VIII")
self.validate_spline(spline, solver)
self.validate_endpoints(spline, initial, final)
def test_target_time_XX(self):
""" Fatal Trajectory Error in simple spline. """
solver = TrajectorySolver(60, 10, 6, 0, -10, -6.0)
initial = Knot(35.570999922343617, 20.000000000263217, 4.5474735088646412e-13, 1660.7976499923104)
final = Knot(2593.1289999999999, 20.0, 0, 1789.7778999999914)
spline = solver.target_time(initial, final, 20)
## self.plot_it(spline, solver, "test_target_time_XX")
self.validate_spline(spline, solver)
self.validate_endpoints(spline, initial, final)
def test_target_time_XIX(self):
""" negative time interval in spline. """
solver = TrajectorySolver(20, 10, 5, 0, -10, -5.0)
initial = Knot(2,19.2506,2.737,3.452)
final = Knot(701.952,20.0,0,39.3653)
spline = solver.target_time(initial, final, 20)
## self.plot_it(spline, solver, "test_target_time_XIX")
self.validate_spline(spline, solver)
self.validate_endpoints(spline, initial, final)
def test_target_time_XVI(self):
"""Problem from testing."""
solver = TrajectorySolver(60, 5, 2.5, 0, -5, -2.5)
initial = Knot(0.0, 0.0, 0, 0.0)
final = Knot(57.926000000000002, 15.0, 0, 7.1870000000000003)
spline = solver.target_time(initial, final)
## self.plot_it(spline, solver, "test_target_time_XVI")
self.validate_spline(spline, solver)
self.validate_endpoints(spline, initial, final)
def test_target_time_XIII(self):
"""Cruise velocity is very near to initial and final velocities."""
solver = TrajectorySolver(15, 5, 2.5, 0, -5, -2.5)
initial = Knot(52.5, 14.99716768771842, 0, 1530.8197971206223)
final = Knot(267.03399999999999, 15.0, 0, 1545.1247333079007)
spline = solver.target_time(initial, final)
## self.plot_it(spline, solver, "test_target_time_XIII")
self.validate_spline(spline, solver)
self.validate_endpoints(spline, initial, final)
def test_target_time_IV(self):
"""Vehicle needs to slow down to achieve target time"""
solver = TrajectorySolver(40, 5, 2.5, 0, -5, -2.5)
initial = Knot(0,40,0,0)
final = Knot(1000,40,0,54)
spline = solver.target_time(initial, final)
## self.plot_it(spline, solver, "test_target_time_IV")
self.validate_spline(spline, solver)
self.validate_endpoints(spline, initial, final)
def test_target_time_XI(self):
"""Problem from testing."""
solver = TrajectorySolver(15, 5, 2.5, 0, -5, -2.5)
initial = Knot(4.8999999999999986, 0.0, 0.0, 81.599999999999994)
final = Knot(309.26081973091823, 15.0, 0, 104.90984383153337)
spline = solver.target_time(initial, final)
## self.plot_it(spline, solver, "test_target_time_XI")
self.validate_spline(spline, solver)
self.validate_endpoints(spline, initial, final)
def test_target_time_X(self):
"""Problem from testing. Initial velocity is very close to ave velocity."""
solver = TrajectorySolver(15, 5, 2.5, 0, -5, -2.5)
initial = Knot(47.529766162611494, 11.907670085181412, 0, 101.08822071657993)
final = Knot(309.26081973091823, 15.0, 0, 122.74317716486676)
spline = solver.target_time(initial, final)
## self.plot_it(spline, solver, "test_target_time_X")
self.validate_spline(spline, solver)
self.validate_endpoints(spline, initial, final)
def test_target_time_V(self):
"""Very short distance, but plenty of time. Does not have a constant acceleration segment."""
solver = TrajectorySolver(40, 5, 2.5, 0, -5, -2.5)
initial = Knot(0,0,0,0)
final = Knot(5,0,0,10)
spline = solver.target_time(initial, final)
## self.plot_it(spline, solver, "test_target_time_V")
self.validate_spline(spline, solver)
self.validate_endpoints(spline, initial, final)
def test_target_time_VII(self):
"""A problem that cropped up during testing."""
solver = TrajectorySolver(60, 5, 2.5, 0, -5, -2.5)
initial = Knot(93.318734132069295, 16.0, 0.0, 5.2)
final = Knot(307.3406064472577, 16.0, 0, 18.95297859206239)
spline = solver.target_time(initial, final)
## self.plot_it(spline, solver, "test_target_time_VII")
self.validate_spline(spline, solver)
self.validate_endpoints(spline, initial, final)
def test_target_time_VI(self):
"""Vehicle needs to exactly maintain current speed to achieve target time"""
solver = TrajectorySolver(40, 5, 2.5, 0, -5, -2.5)
initial = Knot(0,40,0,0)
final = Knot(1000,40,0,25)
spline = solver.target_time(initial, final)
## self.plot_it(spline, solver, "test_target_time_VI")
self.validate_spline(spline, solver)
self.validate_endpoints(spline, initial, final)
def test__no_acceleration_predict_III(self):
"""Has initial vel, no initial accel. Accelerates."""
solver = TrajectorySolver(40, 5, 2.5, 0, -5, -2.5)
initial = Knot(10,2,0,0)
final = Knot(50,10,0,None)
spline = solver._no_acceleration_predict(initial, final)
## self.plot_it(spline, solver, "_no_acceleration_predict_III")
self.validate_spline(spline, solver)
self.validate_endpoints(spline, initial, final, time=False)
def test_target_time_IX(self):
"""Problem from testing. Initial velocity is close to the cruise velocity."""
solver = TrajectorySolver(60, 5, 2.5, 0, -5, -2.5)
initial = Knot(47.529766162611494, 7.8664068437709354, 0, 43.972657903425244)
final = Knot(309.26081973091823, 15.0, 0, 75.376510498199906)
spline = solver.target_time(initial, final)
## self.plot_it(spline, solver, "test_target_time_IX")
self.validate_spline(spline, solver)
self.validate_endpoints(spline, initial, final)