def test_process_spline_II(self):
"""Check that the vehicle only trusts the jerk coefficients to recreate
the spline (not trusting controllers knowledge of vehicle state)."""
spline = CubicSpline(
[50, 53.333333333333336, 73.333333333333343, 110.0, 190.0, 226.66666666666666, 246.66666666666666, 250, 250], # pos
[0, 5.0, 15.0, 20.0, 20.0, 15.0, 5.0, 0, 0], # vel
[0, 5, 5, 0, 0, -5, -5, 0, 0], # accel
[2.5, 0, -2.5, 0, -2.5, 0, 2.5, 0], # jerk
[0, 2.0, 4.0, 6.0, 10.0, 12.0, 14.0, 16.0, 100]) # time
spline_msg = api.Spline()
spline.fill_spline_msg(spline_msg)
# Zero out everything but jerk and times in the message
for poly in spline_msg.polys:
for i in range(1,4): # skip jerk
poly.coeffs[i] = 0
v0 = vehicle.BaseVehicle(0, self.ts0, 50, 0)
v0.process_spline_msg(spline_msg)
# Test that the vehicle's spline matches the original
for q_orig, q_vehicle in zip(spline.q, v0._spline.q):
self.assertAlmostEqual(q_orig, q_vehicle, self.PLACES)
for v_orig, v_vehicle in zip(spline.v, v0._spline.v):
self.assertAlmostEqual(v_orig, v_vehicle, self.PLACES)
for a_orig, a_vehicle in zip(spline.a, v0._spline.a):
self.assertAlmostEqual(a_orig, a_vehicle, self.PLACES)
for coeffs_orig, coeffs_vehicle in zip(spline.coeffs, v0._spline.coeffs):
for x_orig, x_vehicle in zip(coeffs_orig, coeffs_vehicle):
self.assertAlmostEqual(x_orig, x_vehicle, self.PLACES)
def test_updating_trajectory(self):
"""Creates a vehicle, starts the sim with it travelling at a fixed velocity,
then changes the trajectory 11 seconds after the start. When the trajectory
is changed, the vehicle is straddling two track segments."""
v0 = vehicle.BaseVehicle(0, self.ts0, 50, 5) # fixed velocity, 5 m/s
# stop at 50 meters on ts2
spline = CubicSpline(
[2, 6.2324427610773778, 20.693227678868798, 46.258768272424305, 46.67126664464751, 49.999999999978492], # pos
[5, 5.8065992674127145, 9.581117951456692, 5.3923182554918929, 4.9953989633679301, -9.5825569701446511e-12], # vel
[0, 2.0082321422244926, 2.0082321422244926, -4.9976989522066617, -4.9976989522066617, -1.8332002582610585e-12], # accel
[2.5, 0, -2.5, 0, 2.5], # jerk
[11, 11.803292856889797, 13.682815948210798, 16.48518838598326, 16.564608794439177, 18.56368837532111]) # time
spline_msg = api.Spline()
spline.fill_spline_msg(spline_msg)
controller = MockController()
controller.add_cmd(11, v0.process_spline_msg, spline_msg)
Sim.activate(controller, controller.run())
Sim.activate(v0, v0.ctrl_loop())
Sim.simulate(until=20)
self.assertAlmostEqual(v0.pos, 50, self.PLACES)
self.assertTrue(v0.loc is self.ts2)
self.assertAlmostEqual(v0.vel, 0, self.PLACES)
self.assertAlmostEqual(v0.accel, 0, self.PLACES)
self.assertAlmostEqual(v0.tail_pos, 45, self.PLACES)
self.assertTrue(v0.tail_loc is self.ts2)
self.assertTrue(len(self.ts0.vehicles) == 0)
self.assertTrue(len(self.ts1.vehicles) == 0)
self.assertTrue(len(self.ts2.vehicles) == 1)
def test_find_intersection(self):
# spline2 starts behind spline1, but ends in the same place.
spline1 = CubicSpline( # goes to position: 100, then stays forever
[
10, 13.333333333333334, 24.744343897926601, 85.255656102073402,
96.666666666666671, 100, 100
], [0, 5.0, 11.794494717703369, 11.794494717703369, 5.0, 0, 0],
[0, 5, 5, -5, -5, 0, 0], [2.5, 0, -2.5, 0, 2.5, 0], [
0, 2.0, 3.358898943540674, 7.358898943540674,
8.717797887081348, 10.717797887081348, inf
])
spline2 = CubicSpline(
[
0, 3.3333333333333335, 17.507576383774929, 82.492423616225054,
96.666666666666643, 100
], [0, 5.0, 12.912878474779198, 12.912878474779198, 5.0, 0],
[0, 5, 5, -5, -5, 0], [2.5, 0, -2.5, 0, 2.5], [
0, 2.0, 3.5825756949558398, 7.5825756949558398,
9.1651513899116797, 11.16515138991168
])
dist = (spline1.q[0] -
6) - spline2.q[0] # assume spline1's vehicle is 6 meters long
t = spline2.find_intersection(spline1, 0, spline2.t[-1], dist)
self.assertAlmostEqual(
spline1.evaluate(t).pos - 6,
spline2.evaluate(t).pos)
# Test that nothing is returned if past the point of intersection
dist = (spline1.evaluate(t + 0.1).pos - 6) - spline2.evaluate(t +
0.1).pos
t2 = spline2.find_intersection(spline1, t + 0.1, spline2.t[-1], dist)
self.assertEqual(t2, None)
def test_process_spline_I(self):
"""Check that a spline was transferred correctly to the vehicle."""
spline = CubicSpline(
[50, 53.333333333333336, 73.333333333333343, 110.0, 190.0, 226.66666666666666, 246.66666666666666, 250, 250], # pos
[0, 5.0, 15.0, 20.0, 20.0, 15.0, 5.0, 0, 0], # vel
[0, 5, 5, 0, 0, -5, -5, 0, 0], # accel
[2.5, 0, -2.5, 0, -2.5, 0, 2.5, 0], # jerk
[0, 2.0, 4.0, 6.0, 10.0, 12.0, 14.0, 16.0, 100]) # time
spline_msg = api.Spline()
spline.fill_spline_msg(spline_msg)
v0 = vehicle.BaseVehicle(0, self.ts0, 50, 0)
v0.process_spline_msg(spline_msg)
# Test that the vehicle's spline matches the original
for q_orig, q_vehicle in zip(spline.q, v0._spline.q):
self.assertAlmostEqual(q_orig, q_vehicle, self.PLACES)
for v_orig, v_vehicle in zip(spline.v, v0._spline.v):
self.assertAlmostEqual(v_orig, v_vehicle, self.PLACES)
for a_orig, a_vehicle in zip(spline.a, v0._spline.a):
self.assertAlmostEqual(a_orig, a_vehicle, self.PLACES)
for coeffs_orig, coeffs_vehicle in zip(spline.coeffs, v0._spline.coeffs):
for x_orig, x_vehicle in zip(coeffs_orig, coeffs_vehicle):
self.assertAlmostEqual(x_orig, x_vehicle, self.PLACES)
def test_evaluate_sequence(self):
spline = CubicSpline(
[0, 1 / 6, 1], # pos
[0, 1 / 2, 1], # vel
[0, 1, 0], # accel
[1, -1], # jerk
[0, 1, 2]) # time
times = [0, 0.5, 1, 1.25, 1.5, 1.75, 2.0]
knots = spline.evaluate_sequence(times)
expected_accels = [0, 0.5, 1, 0.75, 0.5, 0.25, 0]
for knot, expected in zip(knots, expected_accels):
self.assertEquals(knot.accel, expected)
def test_evaluate(self):
spline = CubicSpline(
[0, 1 / 6, 1], # pos
[0, 1 / 2, 1], # vel
[0, 1, 0], # accel
[1, -1], # jerk
[0, 1, 2]) # time
knot = spline.evaluate(0.5)
self.assertEquals(knot.pos, 1 / 48)
self.assertEquals(knot.vel, 0.125)
self.assertEquals(knot.accel, 0.5)
self.assertEquals(knot.time, 0.5)
def test_append_III(self):
"""Check that append works properly when starting with an empty spline."""
spline = CubicSpline([], [], [], [], [])
spline.coeffs # Trigger the lazy evaluation of coeffs
knot = Knot(11 / 3, 2, 1, 4)
spline.append(knot, None)
self.assertEqual(spline.q, [knot.pos])
self.assertEqual(spline.v, [knot.vel])
self.assertEqual(spline.a, [knot.accel])
self.assertEqual(spline.j, [])
self.assertEqual(spline.t, [knot.time])
self.assertEqual(spline.h, [])
self.polys_coeffs_check(spline)
def test_append_II(self):
"""Similar to test_append, but the evaluation of coeffs is forced prior
to appending the new knot."""
spline = CubicSpline([0, 1 / 6, 1], [0, 1 / 2, 1], [0, 1, 0], [1, -1],
[0, 1, 2])
knot = Knot(11 / 3, 2, 1, 4) # calculated with jerk = 1/2
spline.coeffs # Trigger the lazy evaluation of coeffs
spline.append(knot, 1 / 2)
self.assertEqual(spline.q, [0, 1 / 6, 1, 11 / 3])
self.assertEqual(spline.v, [0, 1 / 2, 1, 2])
self.assertEqual(spline.a, [0, 1, 0, 1])
self.assertEqual(spline.j, [1, -1, 1 / 2])
self.assertEqual(spline.t, [0, 1, 2, 4])
self.assertEqual(spline.h, [1, 1, 2])
self.polys_coeffs_check(spline)
def test_append(self):
spline = CubicSpline([0, 1 / 6, 1], [0, 1 / 2, 1], [0, 1, 0], [1, -1],
[0, 1, 2])
knot = Knot(11 / 3, 2, 1, 4) # calculated with jerk = 1/2
spline.append(knot, 1 / 2)
self.assertEqual(spline.q, [0, 1 / 6, 1, 11 / 3])
self.assertEqual(spline.v, [0, 1 / 2, 1, 2])
self.assertEqual(spline.a, [0, 1, 0, 1])
self.assertEqual(spline.j, [1, -1, 1 / 2])
self.assertEqual(spline.t, [0, 1, 2, 4])
self.assertEqual(spline.h, [1, 1, 2])
self.polys_coeffs_check(spline)
bad_knot = Knot(0, 0, 0, 1) # The time is too early
self.assertRaises(SplineError, spline.append, bad_knot, None)
def test_concat(self):
spline1 = CubicSpline([0, 1 / 6, 1], [0, 1 / 2, 1], [0, 1, 0], [1, -1],
[0, 1, 2])
spline1.coeffs # Trigger the lazy evaluation of coeffs
spline2 = CubicSpline([1, 11 / 3], [1, 2], [0, 1], [1 / 2], [2, 4])
spline2.coeffs # Trigger the lazy evaluation of coeffs
spline = spline1.concat(spline2)
self.assertEqual(spline.q, [0, 1 / 6, 1, 11 / 3])
self.assertEqual(spline.v, [0, 1 / 2, 1, 2])
self.assertEqual(spline.a, [0, 1, 0, 1])
self.assertEqual(spline.j, [1, -1, 1 / 2])
self.assertEqual(spline.t, [0, 1, 2, 4])
self.assertEqual(spline.h, [1, 1, 2])
self.polys_coeffs_check(spline)
self.assertRaises(SplineError, spline2.concat, spline1)
def test_init(self):
spline = CubicSpline([0, 1 / 6, 1], [0, 1 / 2, 1], [0, 1, 0], [1, -1],
[0, 1, 2])
self.assertEqual(spline.q, [0, 1 / 6, 1])
self.assertEqual(spline.v, [0, 0.5, 1])
self.assertEqual(spline.a, [0, 1, 0])
self.assertEqual(spline.j, [1, -1])
self.assertEqual(spline.t, [0, 1, 2])
self.assertEqual(spline.h, [1, 1])
self.polys_coeffs_check(spline)
def make_test_spline_I(self):
return CubicSpline( # velocity peaks midway between the 3rd and 4th knot
[
0, 3.3333333333333335, 49.30209095900485, 150.69790904099514,
196.66666666666663, 200
], # pos
[0, 5.0, 22.015621187164243, 22.015621187164243, 5.0, 0], # vel
[0, 5, 5, -5, -5, 0], # accel
[2.5, 0, -2.5, 0, 2.5], # jerk
[
0, 2.0, 5.4031242374328485, 9.4031242374328485,
12.806248474865697, 14.806248474865697
]) # time
def test_coeffs_II(self):
"""Test with non-zero start time."""
spline = CubicSpline(
[
200, 196.66666666666666, 150.69790904099514,
49.302090959004843, 3.3333333333333428, 0
], # pos
[0, -5.0, -22.015621187164243, -22.015621187164243, -5.0, 0
], # vel
[0, -5, -5, 5, 5, 0], # accel
[-2.5, 0, 2.5, 0, -2.5], # jerk
[
10, 12.0, 15.403124237432849, 19.403124237432849,
22.806248474865697, 24.806248474865697
]) # time
self.polys_coeffs_check(spline)