def test_next_car_state_3(self):
state = CarState([0.0, 0.0, 1.0, 0.0])
control = CarControl([0.0, 0.0])
next_state = new_next_car_state(state, control, friction=0.5, dt=1.0)
self.assertAlmostEqual(next_state.x.numpy(), 0.75)
self.assertAlmostEqual(next_state.y.numpy(), 0.0)
self.assertAlmostEqual(next_state.v.numpy(), 0.5)
self.assertAlmostEqual(next_state.angle.numpy(), 0.0)
def test_next_car_state_batched(self):
state = BatchedCarState([[0.0, 0.0, 1.0, np.pi / 2],
[0.0, 0.0, 1.0, 0.0]])
control = CarControl([0.0, 0.0])
next_state = new_next_car_state(state, control, friction=0.5, dt=1.0)
np.testing.assert_almost_equal(next_state.x.numpy(), [0.0, 0.75])
np.testing.assert_almost_equal(next_state.y, [0.75, 0.0])
np.testing.assert_almost_equal(next_state.v, [0.5, 0.5])
np.testing.assert_almost_equal(next_state.angle, [np.pi / 2, 0.0])
def test_control_invalid_input_0(self):
with self.assertRaises(ValueError):
CarControl([1.0, 0.0, 10.0])
def test_single_control(self):
control = CarControl([1.0, 0.0])
self.assertAlmostEqual(control.acc.numpy(), 1.0)
self.assertAlmostEqual(control.ang_vel.numpy(), 0.0)