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)