def test_target_time_XVII(self): """Problem from testing. Positive starting acceleration and velocity. Leads to negative 0->1 duration.""" solver = TrajectorySolver(15, 10, 5.0, 0, -10, -5.0) initial = Knot(52.503999999999998, 11.51496564563041, 5.6694744752383066, 55.354144791110656) final = Knot(643.97199999999998, 15.0, 0, 95.582584140902242) spline = solver.target_time(initial, final, max_speed=15.0) ## self.plot_it(spline, solver, "test_target_time_XVII") self.validate_spline(spline, solver) self.validate_endpoints(spline, initial, final)
def test_target_position_none_I(self): """Zero endpoints""" solver = TrajectorySolver(30, 5, 2.5) spline = solver.target_position_none(Knot(0,0,0,0), Knot(10,0,0,0)) # self.plot_it(spline, solver) self.validate_spline(spline, solver) self.assertAlmostEqual(spline.q[-1], 10, 5) self.assertAlmostEqual(spline.v[-1], 0, 5) self.assertAlmostEqual(spline.a[-1], 0, 5) self.assertTrue(spline.t[-1] < 6)
def test_target_position_none_III(self): """Travelling in reverse.""" solver = TrajectorySolver(30, 5, 2.5, -30, -5, -2.5) spline = solver.target_position_none(Knot(0,0,0,0), Knot(-10,0,0,0)) ## self.plot_it(spline, solver, "test_target_position_none_III") self.validate_spline(spline, solver) self.assertAlmostEqual(spline.q[-1], -10, 5) self.assertAlmostEqual(spline.v[-1], 0, 5) self.assertAlmostEqual(spline.a[-1], 0, 5) self.assertTrue(spline.t[-1] < 6)
def test_target_velocity_no_delta_v_II(self): """Velocity is already at target, but there's an initial acceleration. Asymmetric jerk limits.""" solver = TrajectorySolver(40, 5, 2.5, 0, -20, -1.25) initial = Knot(0, 20, 5, 0) final = Knot(None, 20, 0, None) spline = solver.target_velocity(initial, final) ## self.plot_it(spline, solver, "test_target_velocity_no_delta_v_II") self.validate_spline(spline, solver) self.validate_endpoints(spline, initial, final, pos=False, time=False)
def test_target_time_increase_vel(self): """Vehicle starts with zero velocity and ends with a velocity that is higher than the 'cruising velocity' required to hit the target time.""" solver = TrajectorySolver(40, 5, 2.5, 0, -5, -2.5) initial = Knot(0,0,0,0) final = Knot(1000, 40, 0, 54) spline = solver.target_time(initial, final) ## self.plot_it(spline, solver, "test_target_time_increase_vel") self.validate_spline(spline, solver) self.validate_endpoints(spline, initial, final)
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_velocity_nonzero_endpoints_I(self): """Initial accel in same direction as initial velocity. Asymmetrical limits""" solver = TrajectorySolver(40, 5, 2.5, 0, -20, -1.25) initial = Knot(10,2,1,0) final = Knot(None,35,3,None) spline = solver.target_velocity(initial, final) ## self.plot_it(spline, solver, "test_target_velocity_nonzero_endpoints_I") self.validate_spline(spline, solver) self.validate_endpoints(spline, initial, final, pos=False, time=False) self.assertTrue(spline.q[-1] > 0) self.assertAlmostEqual(spline.t[-1], 7.56, self.PLACES)
def test_target_position_III(self): """Checks to make sure that it's using the right solver.""" solver = TrajectorySolver(32, 1.0, 2.5, 0, -5.0, -2.5) spline = solver.target_position(Knot(0.0, 10.680000305175781, -4.8299999237060547, 27.727), Knot(57.145143987525579, 0, 0, None)) ## self.plot_it(spline, solver, "test_target_position_III") self.validate_spline(spline, solver) self.assertAlmostEqual(spline.q[-1], 57.145143987525579, 5) self.assertAlmostEqual(spline.v[-1], 0, 5) self.assertAlmostEqual(spline.a[-1], 0, 5) self.assertTrue(spline.t[-1] - spline.t[0] <25)
def test_target_velocity_IV(self): """Asymmetrical limits.""" solver = TrajectorySolver(40, 5, 2.5, 0, -20, -1.25) ### Decelerating, zero at endpoints initial = Knot(0,35,0,0) final = Knot(None, 0, 0, 0) spline = solver.target_velocity(initial, final) ## self.plot_it(spline, solver) self.validate_spline(spline, solver) self.validate_endpoints(spline, initial, final, pos=False, time=False)
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)
def test_target_time_nearly_stops(self): """Initial knot has a positive velocity, and the vehicle needs to almost come to a halt in order to arrive on time. Check that the vehicle does not reverse in order to meet the schedule. """ solver = TrajectorySolver(20, 5, 2.5, 0, -5, -2.5) initial = Knot(0,20,0,0) final = Knot(500,20,0,1000) spline = solver.target_time(initial, final) ## self.plot_it(spline, solver, "test_target_time_nearly_stops") self.validate_spline(spline, solver) self.validate_endpoints(spline, initial, final) self.assertTrue(spline.get_min_velocity() >= 0)
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_V(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, 12.135705947875977, -5.0, 220.824), Knot(76.021737702490753, 0, 0, None)) ## self.plot_it(spline, solver, "test_target_position_V") self.validate_spline(spline, solver) self.assertAlmostEqual(spline.q[-1], 76.021737702490753, 5) self.assertAlmostEqual(spline.v[-1], 0, 5) self.assertAlmostEqual(spline.a[-1], 0, 5) self.assertTrue(spline.t[-1] - spline.t[0] < 30) except OptimizationError: spline = solver._soln_spline ## self.plot_it(spline, solver, "test_target_position_V") self.assertTrue(False)
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_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_position_IV(self): """Test from a real-world solver failure.""" solver = TrajectorySolver(32, 1.0, 2.5, -5.0, -5.0, -2.5) try: spline = solver.target_position(Knot(0, 10.573277473449707, -5.0, 303.644), Knot(68.925505241605777, 0, 0, None)) ## self.plot_it(spline, solver, "test_target_position_IV") self.validate_spline(spline, solver) self.assertAlmostEqual(spline.q[-1], 68.925505241605777, 5) self.assertAlmostEqual(spline.v[-1], 0, 5) self.assertAlmostEqual(spline.a[-1], 0, 5) self.assertTrue(spline.t[-1] - spline.t[0] < 30) except OptimizationError: spline = solver._soln_spline ## self.plot_it(spline, solver, "test_target_position_IV") self.assertTrue(False)
def calc(self): try: self.solver = TrajectorySolver(self.v_max, self.a_max, self.j_max, self.v_min, self.a_min, self.j_min) self.initial = Knot(self.q_i, self.v_i, self.a_i, self.t_i) self.final = Knot(self.q_f, self.v_f, self.a_f, self.t_f) if self.target_type == 'Position': self.spline = self.solver.target_position( self.initial, self.final) elif self.target_type == 'Velocity': self.spline = self.solver.target_velocity( self.initial, self.final) elif self.target_type == 'Acceleration': self.spline = self.solver.target_acceleration( self.initial, self.final) elif self.target_type == 'Time': self.spline = self.solver.target_time(self.initial, self.final) pos = vel = accel = jerk = power = False if "Position" in self.active_plots: pos = True if "Velocity" in self.active_plots: vel = True if "Acceleration" in self.active_plots: accel = True if "Jerk" in self.active_plots: jerk = True if "Power" in self.active_plots: power = True self.plotter = CSplinePlotter(self.spline, self.v_max, self.a_max, self.j_max, self.v_min, self.a_min, self.j_min, mass=self.mass, plot_pos=pos, plot_vel=vel, plot_accel=accel, plot_jerk=jerk, plot_power=power) self.plot_container = self.plotter.container except: self.initial = None self.final = None self.spline = None self.plot_container = Container()
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_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_velocity_II(self): """Accelerating. Doesn't reach a_max""" solver = TrajectorySolver(40, 40, 2.5) ### zero at endpoints spline = solver.target_velocity(Knot(0,0,0,0), Knot(None, 35, 0, 0)) # self.plot_it(spline, solver) self.validate_spline(spline, solver) self.assertTrue(spline.q[-1] > 0) self.assertAlmostEqual(spline.v[-1], 35, self.PLACES) self.assertAlmostEqual(spline.a[-1], 0, self.PLACES) self.assertAlmostEqual(spline.t[-1], 7.4833147735, self.PLACES) # Non-zero endpoints initial = Knot(10,2,1,0) final = Knot(None, 35, 3, 5.883314773) spline = solver.target_velocity(initial, final) # self.plot_it(spline, solver) self.validate_spline(spline, solver) self.validate_endpoints(spline, initial, final, pos=False)
def test_target_velocity_I(self): """Accelerating. Reaches a_max""" solver = TrajectorySolver(40, 5, 2.5) ### zero at endpoints initial = Knot(0,0,0,0) final = Knot(None, 35, 0, 9) spline = solver.target_velocity(initial, final) ## self.plot_it(spline, solver) self.validate_spline(spline, solver) self.validate_endpoints(spline, initial, final, pos=False) ### Unchanging accel spline = solver.target_velocity(Knot(0,0,5,0), Knot(None, 35, 5, 0)) # self.plot_it(spline, solver) self.validate_spline(spline, solver) self.assertTrue(spline.q[-1] > 0) self.assertAlmostEqual(spline.v[-1], 35, self.PLACES) self.assertAlmostEqual(spline.a[-1], 5, self.PLACES) self.assertAlmostEqual(spline.t[-1], 7, 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_velocity_III(self): """Decelerating. Reaches a_max""" solver = TrajectorySolver(40, 5, 2.5) ### zero at endpoints initial = Knot(0,35,0,0) final = Knot(None, 0, 0, 0) spline = solver.target_velocity(initial, final) # self.plot_it(spline, solver) self.validate_spline(spline, solver) self.validate_endpoints(spline, initial, final, pos=False, time=False) # Non-zero endpoints initial = Knot(10,35,1,0) final = Knot(None, 2, -1, 0) spline = solver.target_velocity(initial, final) # self.plot_it(spline, solver) self.validate_spline(spline, solver) self.validate_endpoints(spline, initial, final, pos=False, time=False) self.assertAlmostEqual(spline.t[-1], 8.6799999999, 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_amax_I(self): """Doesn't reach v_max, but does reach a_max""" solver = TrajectorySolver(60, 5, 2.5) # zero endpoints spline = solver.target_position_amax(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], 14.8062484748, self.PLACES) # non-zero endpoints spline = solver.target_position_amax(Knot(0,1,-1,0), Knot(200,2,1,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], 1, self.PLACES) self.assertAlmostEqual(spline.t[-1], 14.98258766288, self.PLACES)
def test_target_position_amax_II(self): """Travelling in reverse""" solver = TrajectorySolver(60, 5, 2.5, -60, -5, -2.5) # zero endpoints spline = solver.target_position_amax(Knot(200,0,0,0), Knot(0,0,0,0)) ## self.plot_it(spline, solver) 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], 14.8062484748, self.PLACES) solver = TrajectorySolver(60, 5, 2.5) # non-zero endpoints spline = solver.target_position_amax(Knot(200,2,1,0), Knot(0,1,-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], 1, self.PLACES) self.assertAlmostEqual(spline.a[-1], -1, self.PLACES) self.assertAlmostEqual(spline.t[-1], 16.451805322, self.PLACES)
class SplineExplorer(traits.HasTraits): """A simple UI to adjust the parameters and view the resulting splines.""" v_min = traits.Float(0) v_max = traits.Float(15) a_min = traits.Float(-5) a_max = traits.Float(5) j_min = traits.Float(-2.5) j_max = traits.Float(2.5) mass = traits.Float(400) q_i = traits.Float v_i = traits.Float a_i = traits.Float t_i = traits.Float q_f = traits.Float(100) v_f = traits.Float(0) a_f = traits.Float(0) t_f = traits.Float(18) plot_names = traits.List( ["Position", "Jerk", "Velocity", "Power", "Acceleration"]) active_plots = traits.List target_type = traits.Enum(('Position', 'Velocity', 'Acceleration', 'Time')) plot_container = traits.Instance(Container) recalculate = menu.Action(name="Recalculate", action="recalc") dump = menu.Action(name="Print", action="dump") save = menu.Action(name="Save", action="save") trait_view = ui.View(ui.HGroup( ui.VGroup( ui.Item(name='target_type', label='Target'), ui.VGroup(ui.Item(name='active_plots', show_label=False, editor=ui.CheckListEditor(cols=3, name='plot_names'), style='custom'), label='Show Plots', show_border=True), ui.VGroup(ui.Item(name='q_i', label='Position'), ui.Item(name='v_i', label='Velocity'), ui.Item(name='a_i', label='Acceleration'), ui.Item(name='t_i', label='Time'), label='Initial Conditions', show_border=True), ui.VGroup(ui.Item( name='q_f', label='Position', enabled_when="target_type not in ('Velocity', 'Acceleration')" ), ui.Item(name='v_f', label='Velocity', enabled_when="target_type != 'Acceleration'"), ui.Item(name='a_f', label='Acceleration'), ui.Item(name='t_f', label='Time', enabled_when="target_type == 'Time'"), label='Final Conditions:', show_border=True), ui.VGroup(ui.Item(name='v_min', label='Min Velocity'), ui.Item(name='v_max', label='Max Velocity'), ui.Item(name='a_min', label='Min Acceleration'), ui.Item(name='a_max', label='Max Acceleration'), ui.Item(name='j_min', label='Min Jerk'), ui.Item(name='j_max', label='Max Jerk'), ui.Item(name='mass', label='Vehicle Mass'), label='Constraints', show_border=True)), ui.Item('plot_container', editor=ComponentEditor(), show_label=False)), title='Cubic Spline Explorer', handler=SEButtonHandler(), buttons=[recalculate, dump, save], resizable=True, width=1000) def __init__(self): super(SplineExplorer, self).__init__() self.active_plots = self.plot_names[:] self.active_plots.remove("Power") self.calc() def calc(self): try: self.solver = TrajectorySolver(self.v_max, self.a_max, self.j_max, self.v_min, self.a_min, self.j_min) self.initial = Knot(self.q_i, self.v_i, self.a_i, self.t_i) self.final = Knot(self.q_f, self.v_f, self.a_f, self.t_f) if self.target_type == 'Position': self.spline = self.solver.target_position( self.initial, self.final) elif self.target_type == 'Velocity': self.spline = self.solver.target_velocity( self.initial, self.final) elif self.target_type == 'Acceleration': self.spline = self.solver.target_acceleration( self.initial, self.final) elif self.target_type == 'Time': self.spline = self.solver.target_time(self.initial, self.final) pos = vel = accel = jerk = power = False if "Position" in self.active_plots: pos = True if "Velocity" in self.active_plots: vel = True if "Acceleration" in self.active_plots: accel = True if "Jerk" in self.active_plots: jerk = True if "Power" in self.active_plots: power = True self.plotter = CSplinePlotter(self.spline, self.v_max, self.a_max, self.j_max, self.v_min, self.a_min, self.j_min, mass=self.mass, plot_pos=pos, plot_vel=vel, plot_accel=accel, plot_jerk=jerk, plot_power=power) self.plot_container = self.plotter.container except: self.initial = None self.final = None self.spline = None self.plot_container = Container() def display(self): self.configure_traits() def get_save_filename(self): """Get a filename from the user via a FileDialog. Returns the filename.""" dialog = FileDialog(action="save as", default_filename="spline_00", wildcard="*.png") dialog.open() if dialog.return_code == OK: return dialog.path def save(self, path): """Save an image of the plot. Does not catch any exceptions.""" # Create a graphics context of the right size win_size = self.plot_container.outer_bounds plot_gc = chaco.PlotGraphicsContext(win_size) #plot_gc.set_fill_color("transparent") # Place the plot component into it plot_gc.render_component(self.plot_container) # Save out to the user supplied filename plot_gc.save(path) def _active_plots_changed(self): self.calc() def _target_type_changed(self): self.calc()
def test_init(self): solver = TrajectorySolver(20, 5, 2.5) self.assertEqual(solver.v_min, 0) self.assertEqual(solver.a_min, -5) self.assertEqual(solver.j_min, -2.5)
def test_target_position_II(self): """Can't backup due to v_min=0, but final position is behind start position.""" solver = TrajectorySolver(20, 5, 2.5, 0, -5, -2.5) self.assertRaises(FatalTrajectoryError, solver.target_position, Knot(100, 0, 0, 0), Knot(90, 0, 0, 0) )