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_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_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_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_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_II(self): """Non-zero endpoints""" solver = TrajectorySolver(40, 5, 2.5, 0, -5, -2.5) initial = Knot(100, 10 , 3, 50) final = Knot(1000, 2, -1, 100) spline = solver.target_time(initial, final) ## self.plot_it(spline, solver, "test_target_time_II") 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_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_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)
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_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_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_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_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_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_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_I(self): """Uses zero for accel and velocity endpoints.""" solver = TrajectorySolver(40, 5, 2.5, 0, -5, -2.5) initial = Knot(0,0,0,0) final = Knot(1000, 0, 0, 54) spline = solver.target_time(initial, final) # approx 20 m/s cruise, ignoring jerk limits ## self.plot_it(spline, solver, "test_target_time_I") self.validate_spline(spline, solver) self.validate_endpoints(spline, initial, final)
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_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_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)
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()