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_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_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_position_X(self):
"""Starts with 20.0001 velocity, ends with 20 velocity."""
solver = TrajectorySolver(60, 10, 6, 0, -10, -6)
initial = Knot(2.00000000000274,20.00000000111,2.273736e-13,1367.1)
final = Knot(997.62,20.0, 0, 1384.79)
spline = solver.target_position(initial, final, 20)
## self.plot_it(spline, solver, "test_target_position_X")
self.validate_spline(spline, solver)
self.validate_endpoints(spline, initial, final, time=False)
def test_target_position_VIII(self):
"""Starts with 0 velocity, ends with max velocity."""
solver = TrajectorySolver(40, 5, 2.5, 0, -5, -2.5)
initial = Knot(0,0,0,0)
final = Knot(500, 40, 0, None)
spline = solver.target_position(initial, final)
## self.plot_it(spline, solver, "test_target_position_VIII")
self.validate_spline(spline, solver)
self.validate_endpoints(spline, initial, final, time=False)
def test_target_position_VII(self):
"""Can this be done without resorting to optimize routine?"""
solver = TrajectorySolver(60, 5, 2.5, 0, -5, -2.5)
initial = Knot(0, 15.5, -1.5, 0)
final = Knot(42.5, 2.4, 0, None)
spline = solver.target_position(initial, final, max_speed=initial.vel)
## self.plot_it(spline, solver, "test_target_position_VII")
self.validate_spline(spline, solver)
self.validate_endpoints(spline, initial, final, time=False)
def test_target_position(self):
"""No change from initial to final"""
solver = TrajectorySolver(20, 5, 2.5)
spline = solver.target_position(Knot(0,0,0,0), Knot(0,0,0,0))
self.assertEqual(spline.q, [0])
self.assertEqual(spline.v, [0])
self.assertEqual(spline.a, [0])
self.assertEqual(spline.j, [])
self.assertEqual(spline.t, [0])
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_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_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)
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()
