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_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_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_III(self):
"""Asymetrical jerk limits."""
solver = TrajectorySolver(20, 5, 2.5, 0, -5, -5)
spline = solver.target_position_vmax(Knot(100, 0, 0, 0), Knot(300, 0, 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], 0, self.PLACES)
self.assertAlmostEqual(spline.a[-1], 0, self.PLACES)
self.assertAlmostEqual(spline.t[-1], 15.9375, self.PLACES)
# non-zero endpoints
spline = solver.target_position_vmax(Knot(100, -2, 2, 0), Knot(300, 2, -4, 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], -4, self.PLACES)
self.assertAlmostEqual(spline.t[-1], 14.4719, self.PLACES)
def test_target_position_vmax_V(self):
"""Endpoints are at limits"""
solver = TrajectorySolver(20, 5, 2.5)
# endpoint at v_max
spline = solver.target_position_vmax(Knot(0, 0, 0, 0), Knot(250, 20, 0, 0))
# self.plot_it(spline, solver)
self.assertAlmostEqual(spline.q[-1], 250, self.PLACES)
self.assertAlmostEqual(spline.v[-1], 20, self.PLACES)
self.assertAlmostEqual(spline.a[-1], 0, self.PLACES)
self.assertAlmostEqual(spline.t[-1], 15.5, self.PLACES)
# endpoint at a_max
spline = solver.target_position_vmax(Knot(0, 0, 0, 0), Knot(250, 5, 5, 0))
# self.plot_it(spline, solver)
self.validate_spline(spline, solver)
self.assertAlmostEqual(spline.q[-1], 250, self.PLACES)
self.assertAlmostEqual(spline.v[-1], 5, self.PLACES)
self.assertAlmostEqual(spline.a[-1], 5, self.PLACES)
self.assertAlmostEqual(spline.t[-1], 20.33333333333, self.PLACES)
def test_target_position_vmax_IV(self):
"""Asymetrical accel limits."""
solver = TrajectorySolver(20, 5, 2.5, 0, -10, -2.5)
# zero endpoints
spline = solver.target_position_vmax(Knot(100, 0, 0, 0), Knot(250, 0, 0, 0))
## self.plot_it(spline, solver)
self.validate_spline(spline, solver)
self.assertAlmostEqual(spline.q[-1], 250, self.PLACES)
self.assertAlmostEqual(spline.v[-1], 0, self.PLACES)
self.assertAlmostEqual(spline.a[-1], 0, self.PLACES)
self.assertAlmostEqual(spline.t[-1], 13.32842712, self.PLACES)
# non-zeo endpoints
spline = solver.target_position_vmax(Knot(100, 6, -2, 0), Knot(300, 5, 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], 5, self.PLACES)
self.assertAlmostEqual(spline.a[-1], 2, self.PLACES)
self.assertAlmostEqual(spline.t[-1], 15.0238958709, self.PLACES)
def test_target_position_vmax_VII(self):
"""Running in reverse with asymetrical limits"""
solver = TrajectorySolver(30, 7.5, 5, -10, -2.5, -1)
# Zero endpoints, negative final position
spline = solver.target_position_vmax(Knot(0, 0, 0, 0), Knot(-250, 0, 0, 0))
## self.plot_it(spline, solver, "test_target_position_vmax_VII")
self.validate_spline(spline, solver)
self.assertAlmostEqual(spline.q[-1], -250, self.PLACES)
self.assertAlmostEqual(spline.v[-1], 0, self.PLACES)
self.assertAlmostEqual(spline.a[-1], 0, self.PLACES)
self.assertAlmostEqual(spline.t[-1], 31.181320796734997, self.PLACES)
