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)